AltIMU-10 V5.Sensors, Github linbarary: minimu-9-ahrs-arduino-master Library using Arduino Due Board and

Hi BrandonM,

Thank you !!

Hi BrandonM,

As per your explanation, if the angle of 0 deg is due North then is it in (0 - 360) degrees scale. Also, I see yaw values in negative, can you please elaborate, if this is right? or the values of Yaw are strictly between 0-360 positive.

!ANG:0.47,0.22,-100.54
!ANG:0.48,0.22,-100.55
!ANG:0.50,0.22,-100.56
!ANG:0.50,0.20,-100.56
!ANG:0.52,0.17,-100.56
!ANG:0.53,0.17,-100.57
!ANG:0.53,0.18,-100.57
!ANG:0.52,0.17,-100.57
!ANG:0.51,0.18,-100.58
!ANG:0.53,0.18,-100.58
!ANG:0.54,0.16,-100.58
!ANG:0.57,0.15,-100.59
!ANG:0.59,0.16,-100.59
!ANG:0.54,0.16,-100.59
!ANG:0.49,0.19,-100.60
!ANG:0.50,0.20,-100.61
!ANG:0.51,0.20,-100.62

Regards,
Veena

Hi BrandonM,

I tried to combine the Altimeter code “LPS” SerialMetric.ino with the one with “MinIMU9AHRS” to get the combined readings for roll, pitch, yaw measurements and altimeter.

The observation is that if I just run the code “MinIMU9AHRS.ino” I see different roll, pitch, yaw measurement and if I run the integrated code “SerialMetric.ino+MinIMU9AHRS.ino” then I get a different measurement.

Can you please let me know what it is the solution to this discrepancy. As both of these code’s use I2C
to communicate it’s tricky to use the details independently, as I am interested in both the Yaw and Altimeter readings at once.

Regards,
Veena

The range for the yaw reading is -180° to 180° (which is based on the atan2 function used).

I do not see any obvious reason why printing data about the pressure reading would change your roll, pitch, and yaw readings, so I suspect there might be an issue with the way you combined the programs. Could you post your modified sketch?

Brandon

Hi BrandonM,

Please find the combined sketch for the for YAW + Altimeter readings.

// Uncomment the following line to use a MinIMU-9 v5 or AltIMU-10 v5. Leave commented for older IMUs (up through v4).
#define IMU_V5

// Uncomment the below line to use this axis definition:
   // X axis pointing forward
   // Y axis pointing to the right
   // and Z axis pointing down.
// Positive pitch : nose up
// Positive roll : right wing down
// Positive yaw : clockwise
int SENSOR_SIGN[9] = {1,1,1,-1,-1,-1,1,1,1}; //Correct directions x,y,z - gyro, accelerometer, magnetometer
// Uncomment the below line to use this axis definition:
   // X axis pointing forward
   // Y axis pointing to the left
   // and Z axis pointing up.
// Positive pitch : nose down
// Positive roll : right wing down
// Positive yaw : counterclockwise
//int SENSOR_SIGN[9] = {1,-1,-1,-1,1,1,1,-1,-1}; //Correct directions x,y,z - gyro, accelerometer, magnetometer

// tested with Arduino Uno with ATmega328 and Arduino Duemilanove with ATMega168

#include <Wire.h>
#include <LPS.h>

LPS ps;

// accelerometer: 8 g sensitivity
// 3.9 mg/digit; 1 g = 256
#define GRAVITY 256  //this equivalent to 1G in the raw data coming from the accelerometer

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

// gyro: 2000 dps full scale
// 70 mdps/digit; 1 dps = 0.07
#define Gyro_Gain_X 0.07 //X axis Gyro gain
#define Gyro_Gain_Y 0.07 //Y axis Gyro gain
#define Gyro_Gain_Z 0.07 //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

// LSM303/LIS3MDL magnetometer calibration constants; use the Calibrate example from
// the Pololu LSM303 or LIS3MDL library to find the right values for your board

//#define M_X_MIN -1000
//#define M_Y_MIN -1000
//#define M_Z_MIN -1000
//#define M_X_MAX +1000
//#define M_Y_MAX +1000
//#define M_Z_MAX +1000


//min: { -4230,  -1995,  -2778}   max: { +2241,  +4235,  +3758}

#define M_X_MIN -4230
#define M_Y_MIN -1995
#define M_Z_MIN -2778
#define M_X_MAX +2241
#define M_Y_MAX +4235
#define M_Z_MAX +3758


#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

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 stores the gyro and accelerometer data
int AN_OFFSET[6]={0,0,0,0,0,0}; //Array that stores the Offset of the sensors

int gyro_x;
int gyro_y;
int gyro_z;
int accel_x;
int accel_y;
int accel_z;
int magnetom_x;
int magnetom_y;
int magnetom_z;
float c_magnetom_x;
float c_magnetom_y;
float c_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  }
};

void setup()
{
  Serial.begin(115200);
  Wire.begin();
  if (!ps.init())
  {
    Serial.println("Failed to autodetect pressure sensor!");
    while (1);
  }
  ps.enableDefault(); 
  pinMode (STATUS_LED,OUTPUT);  // Status LED

  I2C_Init();

  Serial.println("Pololu MinIMU-9 + Arduino AHRS");

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

  Accel_Init();
  Compass_Init();
  Gyro_Init();

  delay(20);

  for(int i=0;i<32;i++)    // We take some readings...
    {
    Read_Gyro();
    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);

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

void loop() //Main Loop
{

float pressure = ps.readPressureInchesHg();
float altitude = ps.pressureToAltitudeFeet(pressure);
float temperature = ps.readTemperatureF();

  Serial.print("p: ");
  Serial.print(pressure);
  Serial.print(" inHg\ta: ");
  Serial.print(altitude);
  Serial.print(" ft\tt: ");
  Serial.print(temperature);
  Serial.println(" deg F");
  delay(100);
  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)
      if (G_Dt > 0.2)
        G_Dt = 0; // ignore integration times over 200 ms
    }
    else
      G_Dt = 0;
    // *** DCM algorithm
    // Data adquisition
    Read_Gyro();   // 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();
  }
}

The delay(100); is almost certainly causing problems, since the code after that is supposed to run at 50Hz (which isn’t possible if your code is blocked from running for 100ms each loop). The frequency is important because the program measures rotation by integrating the gyro readings (rotational rate integrated over time), so if the readings are updated much less often than it expects, that would likely cause error.

The easiest solution would probably be to delete the delay(100); and move your code for printing the pressure, altitude and temperature readings to where the compass data is being read (i.e. inside the if(counter>5) statement). That should print out the LPS readings at 10Hz (the same as it would with a 100ms delay).

Brandon

Hi BrandonM,

Thank you for the response. However, regardless of the delay shouldn’t the readings be similar because the sensor was stationary.

Can you please elaborate why the sensor readings might be affected due to delay. Maybe I am missing something here.

Best Regards,
Veena

Could you describe the behavior of the incorrect readings and how they are different from the readings you get with the unmodified AHRS code?

Even if we would expect the delay issue to not have much effect while the sensor is stationary, it is still likely to cause problems in other circumstances, so I recommend fixing it regardless. Have you tried the modification I suggested, and if so, did it help?

Brandon