Hello everyone!
I’ve been reading this forum for a while (several months) and so far I have been able to solve all my problems by just reading, but now I’ve run into more serious problems and my knowledge level is not that high and I don’t fully understand the threads.
I’ve taken the AHRS code and studied it to produce my own quaternion Kalman filter on top of it. I’ve managed to get it working on Arduino Uno with just one MinIMU but my algorithm uses pretty much the entire memory of the Uno, so I decided that I can easily just buy a better microcontroller and just plug in another MinIMU… I was so wrong…
So just as the title says - I am trying to use several (5 or 6) MinIMU-9 v2 on an Arduino Due. From what I managed to gather, first problem is that they need to be on separate I2C lines (referring to this) and according to this the Due has two SDA and SCL lines, but what happens when I need to add another one? Second problem is that the L3G and LSM303 libraries don’t work with the Due, from what I understand it’s because they have some sort of autodetect functions and the Due has two I2C lines (I absolutely made this up so this is probably not the reason why). But then this guys managed to get them to work, the problem is I don’t really understand how.
So my question is simple: What needs to be done for this to work? My understanding of how exactly the Arduino, MinIMU and I2C communication work is limited, but I think I have the necessary technical background read and learn the parts I need. I am writing here in hope to get some pointers about where I should start since these topics are pretty extensive and I wanted to save some time by starting in the right direction.
This is the code I use for the gyro:
#include <L3G.h>
// L3G4200D gyro is set up at 2000 dps full scale, so therefore the following constants
// 70 mdps/digit; 1 dps = 0.07
#define Gyro_Gain 0.07 //Gyro gain on all axes
#define Gyro_Scaled(x) ((x)*ToRad(Gyro_Gain)) //Return the scaled ADC raw data of the gyro in radians for second
#define ToRad(x) ((x)*0.01745329252) // *pi/180
#define ToDeg(x) ((x)*57.2957795131) // *180/pi
L3G gyro;
void gyro_init(){
gyro.init();
gyro.writeReg(L3G_CTRL_REG1, 0x0F); // normal power mode, all axes enabled, 100 Hz
gyro.writeReg(L3G_CTRL_REG4, 0x20); // 2000 dps full scale
}
void read_gyro(){
gyro.read();
gyro_c[0][0]=(float)(Gyro_Scaled(gyro.g.x)-GYRO_OFF[0][0]);
gyro_c[1][0]=(float)(Gyro_Scaled(gyro.g.y)-GYRO_OFF[1][0]);
gyro_c[2][0]=(float)(Gyro_Scaled(gyro.g.z)-GYRO_OFF[2][0]);
}
And this is the code I use for the compass:
#include <LSM303.h>
// LSM303 accelerometer is set up at 8 g sensitivity, so therefore the following constants
// 3.8 mg/digit; 1 g = 256
#define GRAVITY 256 //this equivalent to 1G in the raw data coming from the accelerometer
LSM303 compass;
void acc_init(){
compass.init();
if (compass.getDeviceType() == LSM303DLHC_DEVICE){
compass.writeAccReg(LSM303_CTRL_REG1_A, 0x47); // normal power mode, all axes enabled, 50 Hz
compass.writeAccReg(LSM303_CTRL_REG4_A, 0x28); // 8 g full scale: FS = 10 on DLHC; high resolution output mode
}
else{
compass.writeAccReg(LSM303_CTRL_REG1_A, 0x27); // normal power mode, all axes enabled, 50 Hz
compass.writeAccReg(LSM303_CTRL_REG4_A, 0x30); // 8 g full scale: FS = 11 on DLH, DLM
}
}
void mag_init(){
compass.writeMagReg(LSM303_MR_REG_M, 0x00); // continuous conversion mode
// 15 Hz default
}
void read_acc(){
compass.readAcc();
acc_c[0][0]=(float)(9.8*compass.a.x/GRAVITY);
acc_c[1][0]=(float)(9.8*compass.a.y/GRAVITY);
acc_c[2][0]=(float)(9.8*compass.a.z/GRAVITY);
}
void read_mag(){
compass.readMag();
mag_c[0][0]=((float)(compass.m.x) );//- (float)(-511)) / ((641) - (-511));
mag_c[1][0]=((float)(compass.m.y) );//- (float)(-621)) / ((564) - (-621));
mag_c[2][0]=((float)(compass.m.z) );//- (float)(-575)) / ((558) - (-575));
}
And this is in the main class:
#include <Wire.h>
#define STATUS_LED 13
#define LENGTH 15
float GYRO_OFF[3][1]={{-0.01918}, {0.03493}, {-0.01011}};
float acc_c[3][1]; // Accelerometer current values
float mag_c[3][1]; //Magnetometer current values
float gyro_c[3][1]; //Gyroscope current values
float dt=0.02;
long timer, timer_old;
void setup(){
Serial.begin(115200);
pinMode (STATUS_LED,OUTPUT);
Wire.begin();
digitalWrite(STATUS_LED,LOW);
delay(500);
acc_init();
mag_init();
gyro_init();
delay(500);
digitalWrite(STATUS_LED,HIGH);
timer = micros();
}
void loop(){
if ((micros()-timer)>=20000){
timer_old = timer;
timer=micros();
if (timer>timer_old)
dt = (timer-timer_old)/2000000.0;
else
dt = 0;
read_acc(); //Get data from the accelerometer
read_mag(); //Get data from the magnetometer
read_gyro(); //Get data from the gyroscope
//Operations with the data
}
I apologise if I haven’t done enough research or if the answer is simply that I need to learn everything about the things I’m having problems with. Any tips and/or opinions are highly appreciated! Thanks!