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

Using Arduino Programming Questions
#1

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 ToRad(x) (x*0.01745329252)  // *pi/180
#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 Gyro_Scaled_X(x) x*ToRad(Gyro_Gain_X) //Return the scaled ADC raw data of the gyro in radians for second
#define Gyro_Scaled_Y(x) x*ToRad(Gyro_Gain_Y) //Return the scaled ADC raw data of the gyro in radians for second
#define Gyro_Scaled_Z(x) x*ToRad(Gyro_Gain_Z) //Return the scaled ADC raw data of the gyro in radians for second

#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 ADC_WARM_CYCLES 50
#define STATUS_LED 13 

int8_t sensors[3] = {1,2,0};  // Map the ADC channels gyro_x, gyro_y, gyro_z
int SENSOR_SIGN[9] = {-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[6]; //array that store the 3 ADC filtered data (gyros)
int AN_OFFSET[6]={0,0,0,0,0,0}; //Array that stores the Offset of the sensors
int ACC[3];          //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 MAG_Heading;

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

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

float errorRollPitch[3]= {0,0,0}; 
float errorYaw[3]= {0,0,0};

unsigned int counter=0;
byte gyro_sat=0;

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


float Temporary_Matrix[3][3]={
  {
    0,0,0  }
  ,{
    0,0,0  }
  ,{
    0,0,0  }
};
 
//ADC variables
volatile uint8_t MuxSel=0;
volatile uint8_t analog_reference;
volatile uint16_t analog_buffer[8];
volatile uint8_t analog_count[8];

void setup()
{ 
  Serial.begin(57600);
  pinMode (STATUS_LED,OUTPUT);  // Status LED
  
  Analog_Reference(DEFAULT); 
  Analog_Init();
  I2C_Init();
  Accel_Init();
  Read_Accel();

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

  digitalWrite(STATUS_LED,LOW);
  delay(1500);
 
  // Magnetometer initialization
  Compass_Init();
  
  // Initialze ADC readings and buffers
  Read_adc_raw();
  delay(20);
  
  for(int i=0;i<32;i++)    // We take some readings...
    {
    Read_adc_raw();
    Read_Accel();
    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[5]-=GRAVITY*SENSOR_SIGN[5];
  
  //Serial.println("Offset:");
  for(int y=0; y<6; y++)
    Serial.println(AN_OFFSET[y]);
  
  delay(2000);
  digitalWrite(STATUS_LED,HIGH);
    
  Read_adc_raw();     // ADC initialization
  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
    // Data adquisition
    Read_adc_raw();   // This read gyro data
    Read_Accel();     // Read I2C accelerometer
    
    if (counter > 5)  // Read compass data at 10Hz... (5 loop runs)
      {
      counter=0;
      Read_Compass();    // Read I2C magnetometer
      Compass_Heading(); // Calculate magnetic heading  
      }
    
    // Calculations...
    Matrix_update(); 
    Normalize();
    Drift_correction();
    Euler_angles();
    // ***
   
    printdata();
    
    //Turn off the LED when you saturate any of the gyros.
    if((abs(Gyro_Vector[0])>=ToRad(300))||(abs(Gyro_Vector[1])>=ToRad(300))||(abs(Gyro_Vector[2])>=ToRad(300)))
      {
      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.

#2

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.

#3

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
	
	ADXL345: Accelerometer
	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 ToRad(x) (x*0.01745329252)  // *pi/180
#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 Gyro_Scaled_X(x) x*ToRad(Gyro_Gain_X) //Return the scaled ADC raw data of the gyro in radians for second
#define Gyro_Scaled_Y(x) x*ToRad(Gyro_Gain_Y) //Return the scaled ADC raw data of the gyro in radians for second
#define Gyro_Scaled_Z(x) x*ToRad(Gyro_Gain_Z) //Return the scaled ADC raw data of the gyro in radians for second

#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 ADC_WARM_CYCLES 50
#define STATUS_LED 13 

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

SoftwareSerial SpeakJetSerial =  SoftwareSerial(rxPin, txPin);

int8_t sensors[3] = {1,2,0};  // Map the ADC channels gyro_x, gyro_y, gyro_z
int SENSOR_SIGN[9] = {-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[6]; //array that store the 3 ADC filtered data (gyros)
int AN_OFFSET[6]={0,0,0,0,0,0}; //Array that stores the Offset of the sensors
int ACC[3];          //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 MAG_Heading;

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

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

float errorRollPitch[3]= {0,0,0}; 
float errorYaw[3]= {0,0,0};

unsigned int counter=0;
byte gyro_sat=0;

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


float Temporary_Matrix[3][3]={
  {
    0,0,0  }
  ,{
    0,0,0  }
  ,{
    0,0,0  }
};
 
//ADC variables
volatile uint8_t MuxSel=0;
volatile uint8_t analog_reference;
volatile uint16_t analog_buffer[8];
volatile uint8_t analog_count[8];

void setup()
{ 
  Serial.begin(57600);
  pinMode (STATUS_LED,OUTPUT);  // Status LED
  
  Analog_Reference(DEFAULT); 
  Analog_Init();
  I2C_Init();
  Accel_Init();
  Read_Accel();

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

  digitalWrite(STATUS_LED,LOW);
  delay(1500);
 
  // Magnetometer initialization
  Compass_Init();
  
  // Initialze ADC readings and buffers
  Read_adc_raw();
  delay(20);
  
  for(int i=0;i<32;i++)    // We take some readings...
    {
    Read_adc_raw();
    Read_Accel();
    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[5]-=GRAVITY*SENSOR_SIGN[5];
  
  //Serial.println("Offset:");
  for(int y=0; y<6; y++)
    Serial.println(AN_OFFSET[y]);
  
  delay(2000);
  digitalWrite(STATUS_LED,HIGH);
    
  Read_adc_raw();     // ADC initialization
  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
    // Data adquisition
    Read_adc_raw();   // This read gyro data
    Read_Accel();     // Read I2C accelerometer
    
    if (counter > 5)  // Read compass data at 10Hz... (5 loop runs)
      {
      counter=0;
      Read_Compass();    // Read I2C magnetometer
      Compass_Heading(); // Calculate magnetic heading  
      }
    
    // Calculations...
    Matrix_update(); 
    Normalize();
    Drift_correction();
    Euler_angles();
    // ***
   
    printdata();
    
    //Turn off the LED when you saturate any of the gyros.
    if((abs(Gyro_Vector[0])>=ToRad(300))||(abs(Gyro_Vector[1])>=ToRad(300))||(abs(Gyro_Vector[2])>=ToRad(300)))
      {
      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);
  }
   
}
#4 
  {
  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.

#5

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?

#6

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.

#7

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

ect.

It says they are “not declared in this scope”

#8

That sounds like you’ve lost the library callout or something along those lines then.

#9

Yeah, turned out to be quite obvious. When I copied the code into the new window to modify it I forgot to transfer over the libraries that were tabbed behind it!

Thanks!

#10

I would still take any suggestions on processing this data, if anyone has worked with this model before (the Sparkfun 9DOF Razor IMU AHRS).

#11