clear;
clc;
imu_interval=0.02;
dcmEst=[1 0 0; 0 1 0; 0 0 1];
gyro=[0 5*0.017453293 2*0.017453293]*imu_interval;%banqueo, cabeceo,guinada
g=[1 -gyro(3) gyro(2); gyro(3) 1 -gyro(1); -gyro(2) gyro(1) 1];
graf(450,4)=zeros;
for n = 1:450
dcmEst=dcmEst*g;
error=-dot(dcmEst(1,:),dcmEst(2,:))*0.5;
x_est=[0 0 0];
y_est=[0 0 0];
x_est = dcmEst(2,:) * error;
y_est = dcmEst(1,:) * error;
dcmEst(1,:) = dcmEst(1,:) + x_est;
dcmEst(2,:) = dcmEst(2,:) + y_est;
dcmEst(3,:) = cross(dcmEst(1,:), dcmEst(2,:));
dcmEst(1,:)=dcmEst(1,:)/norm(dcmEst(1,:));
dcmEst(2,:)=dcmEst(2,:)/norm(dcmEst(2,:));
dcmEst(3,:)=dcmEst(3,:)/norm(dcmEst(3,:));
graf(n,1)=n*imu_interval;
graf(n,2)=dcmEst(2,1);%guinada
graf(n,3)=dcmEst(1,3);%cabeceo
graf(n,4)=dcmEst(3,2);%banqueo
end
figure
hold on
plot(graf(:,1),graf(:,2)*(180/pi),'+b');%guinada
plot(graf(:,1),graf(:,3)*(180/pi),'.r');%cabeceo
plot(graf(:,1),graf(:,4)*(180/pi),'.g');%banqueo
grid
clear;
clc;
imu_interval=0.02;
%La posicion inicial en reposo, los marcos de referencia coincide, por lo
%que la DCM inicial es la matriz identidad
dcmEst=[1 0 0; 0 1 0; 0 0 1];
wx=1; %Velocidad angular en °/seg en cada eje
wy=2;
wz=3;
W=[wx wy wz]*pi/180; %Vector velocidad angular en rad/seg
Theta=W*imu_interval; %Multiplicamos por el tiempo de muestreo para obtener ángulo
%Creamos la matriz de rotacion actual
g=[1 -Theta(3) Theta(2); Theta(3) 1 -Theta(1); -Theta(2) Theta(1) 1];
graf(500,4)=zeros; %Array para gráfico
for n = 1:500 %50 pasos * 0,02seg = 10 segundos
%Actualizamos la DCM girándola de acuerdo al array g
dcmEst=dcmEst*g;
%Tenemos que hacerla orthogonal nuevamente.
%Calculamos el error de ortoganalidad
error=-dot(dcmEst(1,:),dcmEst(2,:))*0.5;
%Repartimos el error escalando el vetor X e Y de la DCM
x_est = dcmEst(2,:) * error;
y_est = dcmEst(1,:) * error;
dcmEst(1,:) = dcmEst(1,:) + x_est;
dcmEst(2,:) = dcmEst(2,:) + y_est;
%Para obtener Z ortogonal, hacemos producto vectorial entre X e Y
dcmEst(3,:) = cross(dcmEst(1,:), dcmEst(2,:));
%Ahora hay que renormalizar cada vector fila de la DCM
dcmEst(1,:)=0.5*(3-dot(dcmEst(1,:),dcmEst(1,:))) * dcmEst(1,:);
dcmEst(2,:)=0.5*(3-dot(dcmEst(2,:),dcmEst(2,:))) * dcmEst(2,:);
dcmEst(3,:)=0.5*(3-dot(dcmEst(3,:),dcmEst(3,:))) * dcmEst(3,:);
graf(n,1)=n*imu_interval;
graf(n,2)=atan2(dcmEst(3,2),dcmEst(3,3)); %guinada
graf(n,3)=-asin(dcmEst(3,1)); %cabeceo
graf(n,4)=atan2(dcmEst(2,1),dcmEst(1,1)); %banqueo
end
figure
hold on
plot(graf(:,1),graf(:,2)*(180/pi),'+b');%guinada
plot(graf(:,1),graf(:,3)*(180/pi),'.r');%cabeceo
plot(graf(:,1),graf(:,4)*(180/pi),'.g');%banqueo
grid
clear;
clc;
%%velocidad de muestreo
imu_interval=0.02;
%%%Colocar en una posicion inicial
banqueoT=90*(pi/180);
cabeceoT=0*(pi/180);
guinadaT=0*(pi/180);
dcmEst=angle2dcm(banqueoT,cabeceoT,guinadaT,'XYZ');%banqueo, cabeceo,guinada
%%%
graf(500,4)=zeros;
x_est=[0 0 0];
y_est=[0 0 0];
%%%dura 10 segundos
for n = 1:500
%Sensado del giroscopo grados/s
vel_gyro=[0 2 0];%banqueo,cabeceo,guinada
vel_gyro=vel_gyro*(pi/180);
%%diferencial de angulo
gyro=vel_gyro*imu_interval;
%%matriz dcm diferencial
g=[1 -gyro(3) gyro(2); gyro(3) 1 -gyro(1); -gyro(2) gyro(1) 1];
%%matriz dcm sumada a la diferencial
dcmEst=dcmEst*g;
error=-dot(dcmEst(1,:),dcmEst(2,:))*0.5;
x_est = dcmEst(2,:) * error;
y_est = dcmEst(1,:) * error;
dcmEst(1,:) = dcmEst(1,:) + x_est;
dcmEst(2,:) = dcmEst(2,:) + y_est;
dcmEst(3,:) = cross(dcmEst(1,:), dcmEst(2,:));
dcmEst(1,:)=dcmEst(1,:)/norm(dcmEst(1,:));
dcmEst(2,:)=dcmEst(2,:)/norm(dcmEst(2,:));
dcmEst(3,:)=dcmEst(3,:)/norm(dcmEst(3,:));
graf(n,1)=n*imu_interval;
%%una vez calculada la DCM,Se calcula la velocidad angular respecto a tierra
veloc_g=dcmEst*(vel_gyro');
%%integro para hallar el angulo respecto a tierra
banqueoT=banqueoT+veloc_g(1,1)*imu_interval;%banqueo
cabeceoT=cabeceoT+veloc_g(2,1)*imu_interval;%cabeceo
guinadaT=guinadaT+veloc_g(3,1)*imu_interval;%guinada
almacen_b(n)=banqueoT;
almacen_c(n)=cabeceoT;
almacen_g(n)=guinadaT;
end
banqueoT=banqueoT*(180/pi)
cabeceoT=cabeceoT*(180/pi)
guinadaT=guinadaT*(180/pi)
hold on
plot(graf(:,1),almacen_g*(180/pi),'+b');%guinada
plot(graf(:,1),almacen_c*(180/pi),'.r');%cabeceo
plot(graf(:,1),almacen_b*(180/pi),'.g');%banqueo
grid
clear;
clc;
imu_interval=0.02;
%dcmEst=[1 0 0; 0 1 0; 0 0 1];
dcmEst1=1;dcmEst2=0;dcmEst3=0;dcmEst4=0;dcmEst5=1;dcmEst6=0;dcmEst7=0;dcmEst8=0;dcmEst9=1;
graf(500,4)=zeros;
banqueoT=0*(pi/180);
cabeceoT=0*(pi/180);
guinadaT=0;
%x_est=[0 0 0];
x_est1=0;x_est2=0;x_est3=0;
%y_est=[0 0 0];
y_est1=0;y_est2=0;y_est3=0;
for n = 1:500
%%velocidad del giroscopo
vel_gyro1=0;vel_gyro2=5;vel_gyro3=5;%banqueo, cabeceo,guinada
%%a radianes
vel_gyro1=vel_gyro1*(pi/180);vel_gyro2=vel_gyro2*(pi/180);vel_gyro3=vel_gyro3*(pi/180);
%gyro=[0 5*0.017453293 5*0.017453293]*imu_interval;%banqueo, cabeceo,guinada
gyro1=vel_gyro1*imu_interval;gyro2=vel_gyro2*imu_interval;gyro3=vel_gyro3*imu_interval;
%g=[1 -gyro(3) gyro(2); gyro(3) 1 -gyro(1); -gyro(2) gyro(1) 1];
g1=1;g2=-gyro3;g3=gyro2;g4=gyro3;g5=1;g6=-gyro1;g7=-gyro2;g8=gyro1;g9=1;
%%%previo al siguiente algoritmo
dcm1=dcmEst1;dcm2=dcmEst2;dcm3=dcmEst3;dcm4=dcmEst4;dcm5=dcmEst5;
dcm6=dcmEst6;dcm7=dcmEst7;dcm8=dcmEst8;dcm9=dcmEst9;
%%%% dcmEst=dcmEst*g;
dcmEst1=dcm1+dcm2*g4+dcm3*g7;
dcmEst2=dcm1*g2+dcm2+dcm3*g8;
dcmEst3=dcm1*g3+dcm2*g6+dcm3;
dcmEst4=dcm4+dcm5*g4+dcm6*g7;
dcmEst5=dcm4*g2+dcm5+dcm6*g8;
dcmEst6=dcm4*g3+dcm5*g6+dcm6;
dcmEst7=dcm7+dcm8*g4+dcm9*g7;
dcmEst8=dcm7*g2+dcm8+dcm9*g8;
dcmEst9=dcm7*g3+dcm8*g6+dcm9;
%error=-dot(dcmEst(1,:),dcmEst(2,:))*0.5;
error=-0.5*(dcmEst1*dcmEst4 + dcmEst2*dcmEst5 +dcmEst3*dcmEst6);
%x_est = dcmEst(2,:) * error;
%y_est = dcmEst(1,:) * error;
x_est1=dcmEst4*error;x_est2=dcmEst5*error;x_est3=dcmEst6*error;
y_est1=dcmEst1*error;y_est2=dcmEst2*error;y_est3=dcmEst3*error;
%%%previo al siguiente algoritmo
dcm1=dcmEst1;dcm2=dcmEst2;dcm3=dcmEst3;dcm4=dcmEst4;dcm5=dcmEst5;
dcm6=dcmEst6;dcm7=dcmEst7;dcm8=dcmEst8;dcm9=dcmEst9;
%dcmEst(1,:) = dcmEst(1,:) + x_est;
dcmEst1=dcm1+x_est1;dcmEst2=dcm2+x_est2;dcmEst3=dcm3+x_est3;
%dcmEst(2,:) = dcmEst(2,:) + y_est;
dcmEst4=dcm4+y_est1;dcmEst5=dcm5+y_est2;dcmEst6=dcm6+y_est3;
%dcmEst(3,:) = cross(dcmEst(1,:), dcmEst(2,:));
dcmEst7=(dcm2*dcm6)-(dcm5*dcm3);
dcmEst8=(dcm4*dcm3)-(dcm6*dcm1);
dcmEst9=(dcm1*dcm5)-(dcm4*dcm2);
%dcmEst(1,:)=dcmEst(1,:)/norm(dcmEst(1,:));
%dcmEst(2,:)=dcmEst(2,:)/norm(dcmEst(2,:));
%dcmEst(3,:)=dcmEst(3,:)/norm(dcmEst(3,:));
norm1=sqrt(dcmEst1*dcmEst1+dcmEst2*dcmEst2+dcmEst3*dcmEst3);
norm2=sqrt(dcmEst4*dcmEst4+dcmEst5*dcmEst5+dcmEst6*dcmEst6);
norm3=sqrt(dcmEst7*dcmEst7+dcmEst8*dcmEst8+dcmEst9*dcmEst9);
dcmEst1=dcmEst1/norm1;dcmEst2=dcmEst2/norm1;dcmEst3=dcmEst3/norm1;
dcmEst4=dcmEst4/norm2;dcmEst5=dcmEst5/norm2;dcmEst6=dcmEst6/norm2;
dcmEst7=dcmEst7/norm3;dcmEst8=dcmEst8/norm3;dcmEst9=dcmEst9/norm3;
%veloc_g=dcmEst*(vel_gyro');
veloc_g1=dcmEst1*vel_gyro1+dcmEst2*vel_gyro2+dcmEst3*vel_gyro3;
veloc_g2=dcmEst4*vel_gyro1+dcmEst5*vel_gyro2+dcmEst6*vel_gyro3;
veloc_g3=dcmEst7*vel_gyro1+dcmEst8*vel_gyro2+dcmEst9*vel_gyro3;
banqueoT=banqueoT+veloc_g1*imu_interval;%banqueo
cabeceoT=cabeceoT+veloc_g2*imu_interval;%cabeceo
guinadaT=guinadaT+veloc_g3*imu_interval;%guinada
almacen_b(n)=banqueoT;
almacen_c(n)=cabeceoT;
almacen_g(n)=guinadaT;
graf(n,1)=n*imu_interval;%%solo sirve para simular en matlab
end
guinadaT*(180/pi)
cabeceoT*(180/pi)
banqueoT*(180/pi)
hold on
plot(graf(:,1),almacen_g*(180/pi),'+b');%guinada
plot(graf(:,1),almacen_c*(180/pi),'.r');%cabeceo
plot(graf(:,1),almacen_b*(180/pi),'.g');%banqueo
grid
"elgarbe" tienes una pequeña palabra que esta al revés:
"Los ángulos de Euler (Pitch o cabeceo, Roll o giñada y Yaw o banqueo)"
es "Roll o banqueo" y "Yaw o guiñada"
Saludos
#ifndef MACROUTIL_H
#define MACROUTIL_H
//---------------------------------------------------------------------
// UTIL MACROS
//---------------------------------------------------------------------
#include<math.h>
#define MIN(A,B) (((A)<(B)) ? (A) : (B) )
#define MAX(A,B) (((A)>(B)) ? (A) : (B) )
#define PI 3.141592653589793238462643383279502884197f
double squared(float x){
return x*x;
}
double atan2deg(float y, float x){
if(fabs(y) < 0.1 && fabs(x) < 0.1)
return 0.0; //avoid atan2 returning meaningless numbers
else
return atan2(y,x)*180.0 / PI;
}
float low_pass_filter(float vNew,float vPrev,float factor){
return (vPrev*factor + vNew) / ( 1 + factor);
}
short make_word(unsigned char HB, unsigned char LB){
return ((HB << 8) | LB);
}
void timer0_init(){
// Inicializamos el TIMER 0
LPC_TIM0->MR0 = TIMER0_INTERVAL; //Match register 0
LPC_TIM0->MCR = 3; //Interrumpe y resetea el contador cuando TC alcanza MR0
NVIC_EnableIRQ(TIMER0_IRQn); //Macro en core_cm3 para habilitar una interrupcion
//TIMER0_IRQn sale del LPC17xx.h y es el número de la int del timer
// Deshabilitamos el Timer 0
LPC_TIM1->TCR = 0;
}
void timer0_enable(){
// Habilitamos el Timer 0
LPC_TIM0->TC=0;
LPC_TIM0->TCR = 1;
}
void print_dcm(float vector[3][3]){
uart2_printDouble(vector[0][0], 3);
UART2_Sendchar(',');
uart2_printDouble(vector[0][1], 3);
UART2_Sendchar(',');
uart2_printDouble(vector[0][2], 3);
UART2_Sendchar(',');
uart2_printDouble(vector[1][0], 3);
UART2_Sendchar(',');
uart2_printDouble(vector[1][1], 3);
UART2_Sendchar(',');
uart2_printDouble(vector[1][2], 3);
UART2_Sendchar(',');
uart2_printDouble(vector[2][0], 3);
UART2_Sendchar(',');
uart2_printDouble(vector[2][1], 3);
UART2_Sendchar(',');
uart2_printDouble(vector[2][2], 3);
UART2_Sendchar(',');
uart2_printDouble(imu_interval_ms*1000, 3);
UART2_PrintString ("\r\n");
}
#endif
#ifndef VECTOR3D__H
#define VECTOR3D__H
#include <math.h>
//get modulus of a 3d vector sqrt(x^2+y^2+y^2)
float vector3d_modulus(float* vector){
static float R;
R = vector[0]*vector[0];
R += vector[1]*vector[1];
R += vector[2]*vector[2];
return sqrt(R);
}
//convert vector to a vector with same direction and modulus 1
void vector3d_normalize(float* vector){
static float R;
R = vector3d_modulus(vector);
vector[0] /= R;
vector[1] /= R;
vector[2] /= R;
}
//calcuate vector dot-product c = a . b
float vector3d_dot(float* a,float* b){
return a[0]*b[0]+a[1]*b[1]+a[2]*b[2];
}
//calcuate vector cross-product c = a x b
void vector3d_cross(float a[3],float b[3], float c[3]){
c[0] = a[1]*b[2] - a[2]*b[1];
c[1] = a[2]*b[0] - a[0]*b[2];
c[2] = a[0]*b[1] - a[1]*b[0];
}
//calcuate vector scalar-product n = s x a
void vector3d_scale(float s, float* a , float* b){
b[0] = s*a[0];
b[1] = s*a[1];
b[2] = s*a[2];
}
//calcuate vector sum c = a + b
void vector3d_add(float* a , float* b, float* c){
c[0] = a[0] + b[0];
c[1] = a[1] + b[1];
c[2] = a[2] + b[2];
}
//creates equivalent skew symetric matrix plus identity
//for v = {x,y,z} returns
// m = {{1,-z,y}
// {z,1,-x}
// {-y,x,1}}
void vector3d_skew_plus_identity(float *v,float* m){
m[0*3+0]=1;
m[0*3+1]=-v[2];
m[0*3+2]=v[1];
m[1*3+0]=v[2];
m[1*3+1]=1;
m[1*3+2]=-v[0];
m[2*3+0]=-v[1];
m[2*3+1]=v[0];
m[2*3+2]=1;
}
#endif
#ifndef L3G4200D_H_
#define L3G4200D_H_
#include "i2c.h"
extern volatile uint8_t I2CMasterBuffer[I2C_PORT_NUM][BUFSIZE];
extern volatile uint8_t I2CSlaveBuffer[I2C_PORT_NUM][BUFSIZE];
extern volatile uint32_t I2CReadLength[I2C_PORT_NUM];
extern volatile uint32_t I2CWriteLength[I2C_PORT_NUM];
#define L3G4200D_READ_ADDR 0xD3
#define L3G4200D_WRITE_ADDR 0xD2
#define L3G4200D_DATA_ADDR 0x28
#define L3G4200D_WHO_AM_I 0x0F
#define L3G4200D_CTRL_REG1 0x20
#define L3G4200D_CTRL_REG2 0x21
#define L3G4200D_CTRL_REG3 0x22
#define L3G4200D_CTRL_REG4 0x23
#define L3G4200D_CTRL_REG5 0x24
#define L3G4200D_REFERENCE 0x25
#define L3G4200D_OUT_TEMP 0x26
#define L3G4200D_STATUS_REG 0x27
#define L3G4200D_OUT_X_L 0x28
#define L3G4200D_OUT_X_H 0x29
#define L3G4200D_OUT_Y_L 0x2A
#define L3G4200D_OUT_Y_H 0x2B
#define L3G4200D_OUT_Z_L 0x2C
#define L3G4200D_OUT_Z_H 0x2D
#define L3G4200D_FIFO_CTRL_REG 0x2E
#define L3G4200D_FIFO_SRC_REG 0x2F
#define L3G4200D_INT1_CFG 0x30
#define L3G4200D_INT1_SRC 0x31
#define L3G4200D_INT1_THS_XH 0x32
#define L3G4200D_INT1_THS_XL 0x33
#define L3G4200D_INT1_THS_YH 0x34
#define L3G4200D_INT1_THS_YL 0x35
#define L3G4200D_INT1_THS_ZH 0x36
#define L3G4200D_INT1_THS_ZL 0x37
#define L3G4200D_INT1_DURATION 0x38
#define PORT_USED 1
void L3G4200D_init();
unsigned char L3G4200D_read(unsigned char reg);
unsigned char L3G4200D_write(unsigned char reg_address, unsigned char value);
//short make_word(unsigned char HB, unsigned char LB);
char L3G4200D_read_data();
void L3G4200D_init(){
L3G4200D_write(L3G4200D_CTRL_REG1, 0b00001111); // 100Hz, 12.5 CO, all axis enable
L3G4200D_write(L3G4200D_CTRL_REG4, 0x20); // 0x00 250 dps, 0x10 500 dps, 0x20 2000dps
}
unsigned char L3G4200D_read(unsigned char reg){
I2CWriteLength[PORT_USED] = 2;
I2CReadLength[PORT_USED] = 1;
I2CMasterBuffer[PORT_USED][0] = L3G4200D_WRITE_ADDR;
I2CMasterBuffer[PORT_USED][1] = reg;
I2CMasterBuffer[PORT_USED][2] = L3G4200D_READ_ADDR;
I2CEngine( PORT_USED );
return(I2CSlaveBuffer[PORT_USED][0]);
}
unsigned char L3G4200D_write(unsigned char reg_address, unsigned char value){
I2CWriteLength[PORT_USED] = 3;
I2CReadLength[PORT_USED] = 0;
I2CMasterBuffer[PORT_USED][0] = L3G4200D_WRITE_ADDR;
I2CMasterBuffer[PORT_USED][1] = reg_address;
I2CMasterBuffer[PORT_USED][2] = value;
return(I2CEngine( PORT_USED ));
}
char L3G4200D_read_data(){
char result=0;
I2CWriteLength[PORT_USED] = 2;
I2CReadLength[PORT_USED] = 6;
I2CMasterBuffer[PORT_USED][0] = L3G4200D_WRITE_ADDR;
I2CMasterBuffer[PORT_USED][1] = (L3G4200D_DATA_ADDR | (1<<7));
I2CMasterBuffer[PORT_USED][2] = L3G4200D_READ_ADDR;
result=I2CEngine( PORT_USED );
adcAvg[3] = make_word(I2CSlaveBuffer[PORT_USED][1], I2CSlaveBuffer[PORT_USED][0]);
adcAvg[4] = make_word(I2CSlaveBuffer[PORT_USED][3], I2CSlaveBuffer[PORT_USED][2]);
adcAvg[5] = make_word(I2CSlaveBuffer[PORT_USED][5], I2CSlaveBuffer[PORT_USED][4]);
return(result);
}
void L3G4200D_GetBiass(void){
int i;
for (i = 0; i < SAMPLESS_BIASS; i += 1) {
L3G4200D_read_data();
biass_X += adcAvg[3];
biass_Y += adcAvg[4];
biass_Z += adcAvg[5];
delay_ms(1);
}
biass_X /= SAMPLESS_BIASS;
biass_Y /= SAMPLESS_BIASS;
biass_Z /= SAMPLESS_BIASS;
}
#endif /* L3G4200D_H_ */
#ifndef CONFIG_H
#define CONFIG_H
struct {
unsigned char accInv[3]; // invert accl input (for example due to physical mounting position)
double accOffs[3]; // accl output at 0g in ADC units
double accSens[3]; // accl input sensitivity in ADC/g
unsigned char gyroInv[3]; // invert gyro input (for example due to physical mounting position)
double gyroOffs[3]; // gyro zero rate output in ADC @ 0 deg/s;
double gyroSens[3]; // gyro input sensitivity ADC/(deg/ms)
} config;
void config_default(){
config.gyroOffs[0] = 0;
config.gyroOffs[1] = 0;
config.gyroOffs[2] = 0;
config.gyroSens[0] = GYRO_X_SCALE * PI/180;
config.gyroSens[1] = GYRO_Y_SCALE * PI/180;
config.gyroSens[2] = GYRO_Z_SCALE * PI/180;
config.accInv[0] = 0;
config.accInv[1] = 0;
config.accInv[2] = 0;
config.gyroInv[0] = 0;
config.gyroInv[1] = 0;
config.gyroInv[2] = 0;
}
#endif
#ifndef CALIBRATE_H
#define CALIBRATE_H
/*********** GYRO CALIBRATION ********/
void calibrate_gyro(){
L3G4200D_GetBiass();
config.gyroOffs[0] = biass_X;
config.gyroOffs[1] = biass_Y;
config.gyroOffs[2] = biass_Z;
}
#endif
#ifndef IMU__H
#define IMU__H
/*
How to use this module in other projects.
Input variables are:
adcAvg[0..5] ADC readings of 3 axis accelerometer and 3 axis gyroscope (they are calculated in the background by adcutil.h)
interval_us - interval in microseconds since last call to imu_update
Output variables are:
DcmEst[0..2] which are the direction cosine of the X,Y,Z axis
First you must initialize the module with:
imu_init();
Then call periodically every 5-20ms:
imu_update();
it is assumed that you also update periodicall the adcAvg[0..5] array
*/
//-------------------------------------------------------------------
// Globals
//-------------------------------------------------------------------
unsigned int imu_sequence = 0; //incremented on each call to imu_update
float dcmEst[3][3] = {{1,0,0},{0,1,0},{0,0,1}}; //estimated DCM matrix
//-------------------------------------------------------------------
//Get gyro reading (rate of rotation expressed in deg/ms)
//-------------------------------------------------------------------
float getGyroOutput(unsigned char w){
static float tmpf; //temporary variable for complex calculations
tmpf = adcAvg[3+w] - config.gyroOffs[w]; //remove offset
tmpf *= config.gyroSens[w]; //divide by sensitivity
if( config.gyroInv[w]) tmpf = - tmpf; //invert axis value if needed
return tmpf;
}
//bring dcm matrix in order - adjust values to make orthonormal (or at least closer to orthonormal)
void dcm_orthonormalize(float dcm[3][3]){
//err = X . Y , X = X - err/2 * Y , Y = Y - err/2 * X (DCMDraft2 Eqn.19)
float err = vector3d_dot((float*)(dcm[0]),(float*)(dcm[1]));
float delta[2][3];
vector3d_scale(-err/2,(float*)(dcm[1]),(float*)(delta[0]));
vector3d_scale(-err/2,(float*)(dcm[0]),(float*)(delta[1]));
vector3d_add((float*)(dcm[0]),(float*)(delta[0]),(float*)(dcm[0]));
vector3d_add((float*)(dcm[1]),(float*)(delta[1]),(float*)(dcm[1]));
//Z = X x Y (DCMDraft2 Eqn. 20) ,
vector3d_cross((float*)(dcm[0]),(float*)(dcm[1]),(float*)(dcm[2]));
//re-nomralization
vector3d_normalize((float*)(dcm[0]));
vector3d_normalize((float*)(dcm[1]));
vector3d_normalize((float*)(dcm[2]));
}
//rotate DCM matrix by a small rotation given by angular rotation vector w
//see http://gentlenav.googlecode.com/files/DCMDraft2.pdf
void dcm_rotate(float dcm[3][3], float w[3]){
//float W[3][3];
//creates equivalent skew symetric matrix plus identity matrix
//vector3d_skew_plus_identity((float*)w,(float*)W);
//float dcmTmp[3][3];
//matrix_multiply(3,3,3,(float*)W,(float*)dcm,(float*)dcmTmp);
int i;
float dR[3];
//update matrix using formula R(t+1)= R(t) + dR(t) = R(t) + w x R(t)
for(i=0;i<3;i++){
vector3d_cross(w,dcm[i],dR);
vector3d_add(dcm[i],dR,dcm[i]);
}
//make matrix orthonormal again
dcm_orthonormalize(dcm);
}
//-------------------------------------------------------------------
// imu_init
//-------------------------------------------------------------------
unsigned int count250us_prev;
unsigned int count250us;
void imu_init(){
count250us_prev=count250us;
}
//-------------------------------------------------------------------
// imu_update
//-------------------------------------------------------------------
void imu_update(){
int i;
imu_sequence++;
//interval since last call
imu_interval_ms = 0.00025*(float)(count250us-count250us_prev);
count250us_prev = count250us;
//---------------
// I,J,K unity vectors of global coordinate system I-North,J-West,K-zenith
// i,j,k unity vectors of body's coordiante system i-"nose", j-"left wing", k-"top"
//---------------
// [I.i , I.j, I.k]
// DCM = [J.i , J.j, J.k]
// [K.i , K.j, K.k]
//---------------
//dcmEst
//---------------
//gyro rate direction is usually specified (in datasheets) as the device's(body's) rotation
//about a fixed earth's (global) frame, if we look from the perspective of device then
//the global vectors (I,K,J) rotation direction will be the inverse
float w[3]; //gyro rates (angular velocity of a global vector in local coordinates)
w[0] = -getGyroOutput(0); //rotation rate about accelerometer's X axis (GY output) in rad/ms
w[1] = -getGyroOutput(1); //rotation rate about accelerometer's Y axis (GX output) in rad/ms
w[2] = -getGyroOutput(2); //rotation rate about accelerometer's Z axis (GZ output) in rad/ms
for(i=0;i<3;i++){
w[i] *= imu_interval_ms; //scale by elapsed time to get angle in radians
}
dcm_rotate(dcmEst,w);
}
#endif
#ifdef __USE_CMSIS
#include "LPC17xx.h"
#endif
volatile uint32_t msTicks;
__INLINE static void delay_ms (uint32_t delayTicks) {
uint32_t currentTicks;
currentTicks = msTicks; // read current tick counter
while ((msTicks - currentTicks) < delayTicks);
}
#define SAMPLESS_BIASS 1000
//TC = T * SystemCoreClok/PCLK_TIMER0 con T en segundos
//PCLK_TIMER0 = PCLK_periph/4;
#define usTim0 250
#define TIMER0_INTERVAL 25*usTim0
#define GYRO_X_SCALE 0.079 //Sensibilidad en cada eje
#define GYRO_Y_SCALE 0.079 //extraído del datasheet
#define GYRO_Z_SCALE 0.079
#define ACEL_X_SCALE 0.003135 //Sensibilidad en cada eje
#define ACEL_Y_SCALE 0.003306 //de la calibracion
#define ACEL_Z_SCALE 0.003311
//short accel_X, accel_Y, accel_Z;
//short gyro_X, gyro_Y, gyro_Z;
double biass_X, biass_Y, biass_Z;
float imu_interval_ms = 0; //interval since last call to imu_update
//char res_a=0;
//char res_g=0;
//
double adcAvg[6]={0.0,0.0,0.0,0.0,0.0,0.0}; //Primeros 3 para Acc y despues Gyro
//---------------------------------------------------------------------
// LIBS
//---------------------------------------------------------------------
#include <cr_section_macros.h>
#include <NXP/crp.h>
#include <math.h>
#include "uart2.h"
#include "macroutil.h"
#include <stdio.h>
#include "i2c.h"
#include "L3G4200D.h"
#include "config.h"
#include "vector3d.h"
#include "calibrate.h"
#include "imu.h"
float timeStep = 0.010; //Tiempo que tiene que durar el main
char countfor5ms=0; //this is used in IMU module
char f_leer_data=0;
//Handler de la interrupcion del Timer 0. Sucede cada 250uSeg
void TIMER0_IRQHandler (void)
{
LPC_TIM0->IR = 1; /* clear interrupt flag */
count250us++;
if(++countfor5ms==20){
f_leer_data=1;
countfor5ms=0;
}
}
void SysTick_Handler(void) {
msTicks++; /* increment counter necessary in Delay() */
}
/*******************************************************************************
** Main Function main()
*******************************************************************************/
int main (void){
// int i=0;
uint32_t timer=0;
uint32_t timer2=0;
//Configuro el SysTick para que interrumpa cada 1 mseg
if (SysTick_Config(SystemCoreClock / 1000)) {
while (1);
}
//---------------------------------------------------------------------
// CONFIGURATION
//---------------------------------------------------------------------
config_default(); //Cargo una configuracion fija. Ver de grabar en la flash.
//---------------------------------------------------------------------
// UART
//---------------------------------------------------------------------
UART2_Init(115200); // Inicializo el UART a 115200
UART2_PrintString ("\r\nDCM con Giroscopo L3G4200D\r\n");
UART2_PrintString ("\r\nDCM, tiempo de actualizacion DCM\r\n");
//---------------------------------------------------------------------
// I2C
//---------------------------------------------------------------------
I2C1Init(); /* initialize I2c1 */
L3G4200D_init();
timer0_init();
//---------------------------------------------------------------------
// CALIBRATION
//---------------------------------------------------------------------
calibrate_gyro();
//---------------------------------------------------------------------
// IMU
//---------------------------------------------------------------------
imu_init();
timer0_enable();
timer = msTicks; //get a start value to determine the time the loop takes
timer2 = msTicks;
while ( 1 ){
//Ya pasaron 5mseg por lo que leo los sensores
if(f_leer_data){
L3G4200D_read_data(); //los sensores.
f_leer_data=0;
}
if((msTicks-timer) >= 10 ){
timer=msTicks;
imu_update();
}
if((msTicks-timer2) >= 500){
timer2=msTicks;
print_dcm(dcmEst);
}
}
}
#ifndef CONFIG_H
#define CONFIG_H
struct {
unsigned char accInv[3]; // invert accl input (for example due to physical mounting position)
double accOffs[3]; // accl output at 0g in ADC units
double accSens[3]; // accl input sensitivity in ADC/g
unsigned char gyroInv[3]; // invert gyro input (for example due to physical mounting position)
double gyroOffs[3]; // gyro zero rate output in ADC @ 0 deg/s;
double gyroSens[3]; // gyro input sensitivity ADC/(deg/ms)
} config;
void config_default(){
config.accOffs[0] = 0;
config.accOffs[1] = 0;
config.accOffs[2] = 0;
config.accSens[0] = ACEL_X_SCALE;
config.accSens[1] = ACEL_Y_SCALE;
config.accSens[2] = ACEL_Z_SCALE;
config.gyroOffs[0] = 0;
config.gyroOffs[1] = 0;
config.gyroOffs[2] = 0;
config.gyroSens[0] = GYRO_X_SCALE * PI/180;
config.gyroSens[1] = GYRO_Y_SCALE * PI/180;
config.gyroSens[2] = GYRO_Z_SCALE * PI/180;
config.accInv[0] = 0;
config.accInv[1] = 0;
config.accInv[2] = 0;
config.gyroInv[0] = 0;
config.gyroInv[1] = 0;
config.gyroInv[2] = 0;
}
#endif
#ifndef CALIBRATE_H
#define CALIBRATE_H
/*********** GYRO CALIBRATION ********/
void calibrate_gyro(){
L3G4200D_GetBiass();
config.gyroOffs[0] = biass_X;
config.gyroOffs[1] = biass_Y;
config.gyroOffs[2] = biass_Z;
}
/*********** ACCEL CALIBRATION ********/
void calibrate_acc(){
ADXL345_GetBiass();
config.accOffs[0] = biass_X;
config.accOffs[1] = biass_Y;
config.accOffs[2] = biass_Z;
}
#endif
#ifndef IMU__H
#define IMU__H
/*
How to use this module in other projects.
Input variables are:
adcAvg[0..5] ADC readings of 3 axis accelerometer and 3 axis gyroscope (they are calculated in the background by adcutil.h)
interval_us - interval in microseconds since last call to imu_update
Output variables are:
DcmEst[0..2] which are the direction cosine of the X,Y,Z axis
First you must initialize the module with:
imu_init();
Then call periodically every 5-20ms:
imu_update();
it is assumed that you also update periodicall the adcAvg[0..5] array
*/
#define ACC_WEIGHT_MAX 0.00 //maximum accelerometer weight in accelerometer-gyro fusion formula
//this value is tuned-up experimentally: if you get too much noise - decrease it
//if you get a delayed response of the filtered values - increase it
//starting with a value of 0.01 .. 0.05 will work for most sensors
//-------------------------------------------------------------------
// Globals
//-------------------------------------------------------------------
unsigned int imu_sequence = 0; //incremented on each call to imu_update
float dcmEst[3][3] = {{1,0,0},{0,1,0},{0,0,1}}; //estimated DCM matrix
//-------------------------------------------------------------------
//Get accelerometer reading (accelration expressed in g)
//-------------------------------------------------------------------
float getAcclOutput(unsigned char w){
static float tmpf; //temporary variable for complex calculations
tmpf = adcAvg[w] - config.accOffs[w]; //remove offset
tmpf *= config.accSens[w]; //divide by sensitivity
if(config.accInv[w]) tmpf = - tmpf; //invert axis value if needed
return tmpf;
}
//-------------------------------------------------------------------
//Get gyro reading (rate of rotation expressed in deg/ms)
//-------------------------------------------------------------------
float getGyroOutput(unsigned char w){
static float tmpf; //temporary variable for complex calculations
tmpf = adcAvg[3+w] - config.gyroOffs[w]; //remove offset
tmpf *= config.gyroSens[w]; //divide by sensitivity
if( config.gyroInv[w]) tmpf = - tmpf; //invert axis value if needed
return tmpf;
}
//bring dcm matrix in order - adjust values to make orthonormal (or at least closer to orthonormal)
void dcm_orthonormalize(float dcm[3][3]){
//err = X . Y , X = X - err/2 * Y , Y = Y - err/2 * X (DCMDraft2 Eqn.19)
float err = vector3d_dot((float*)(dcm[0]),(float*)(dcm[1]));
float delta[2][3];
vector3d_scale(-err/2,(float*)(dcm[1]),(float*)(delta[0]));
vector3d_scale(-err/2,(float*)(dcm[0]),(float*)(delta[1]));
vector3d_add((float*)(dcm[0]),(float*)(delta[0]),(float*)(dcm[0]));
vector3d_add((float*)(dcm[1]),(float*)(delta[1]),(float*)(dcm[1]));
//Z = X x Y (DCMDraft2 Eqn. 20) ,
vector3d_cross((float*)(dcm[0]),(float*)(dcm[1]),(float*)(dcm[2]));
//re-nomralization
vector3d_normalize((float*)(dcm[0]));
vector3d_normalize((float*)(dcm[1]));
vector3d_normalize((float*)(dcm[2]));
}
//rotate DCM matrix by a small rotation given by angular rotation vector w
//see http://gentlenav.googlecode.com/files/DCMDraft2.pdf
void dcm_rotate(float dcm[3][3], float w[3]){
//float W[3][3];
//creates equivalent skew symetric matrix plus identity matrix
//vector3d_skew_plus_identity((float*)w,(float*)W);
//float dcmTmp[3][3];
//matrix_multiply(3,3,3,(float*)W,(float*)dcm,(float*)dcmTmp);
int i;
float dR[3];
//update matrix using formula R(t+1)= R(t) + dR(t) = R(t) + w x R(t)
for(i=0;i<3;i++){
vector3d_cross(w,dcm[i],dR);
vector3d_add(dcm[i],dR,dcm[i]);
}
//make matrix orthonormal again
dcm_orthonormalize(dcm);
}
//-------------------------------------------------------------------
// imu_init
//-------------------------------------------------------------------
unsigned int count250us_prev;
unsigned int count250us;
void imu_init(){
count250us_prev=count250us;
}
//-------------------------------------------------------------------
// imu_update
//-------------------------------------------------------------------
#define ACC_WEIGHT 0.01 //accelerometer data weight relative to gyro's weight of 1
#define ACC_ERR_MAX 0.3 //maximum accelerometer errror relative to 1g , when error exceeds this value accelerometer weight becomes 0
//this helps reject external accelerations (non-gravitational innertial forces caused by device acceleration)
float imu_interval_ms = 0; //interval since last call to imu_update
float Kacc[3]; //K(b) vector according to accelerometer in body's coordinates
void imu_update(){
int i;
imu_sequence++;
//interval since last call
imu_interval_ms = 0.00025*(float)(count250us-count250us_prev);
count250us_prev = count250us;
//---------------
// I,J,K unity vectors of global coordinate system I-North,J-West,K-zenith
// i,j,k unity vectors of body's coordiante system i-"nose", j-"left wing", k-"top"
//---------------
// [I.i , I.j, I.k]
// DCM = [J.i , J.j, J.k]
// [K.i , K.j, K.k]
//---------------
//Acelerometer
//---------------
//Accelerometer measures gravity vector G in body coordinate system
//Gravity vector is the reverse of K unity vector of global system expressed in local coordinates
//K vector coincides with the z coordinate of body's i,j,k vectors expressed in global coordinates (K.i , K.j, K.k)
//Acc can estimate global K vector(zenith) measured in body's coordinate systems (the reverse of gravitation vector)
Kacc[0] = getAcclOutput(0);
Kacc[1] = getAcclOutput(1);
Kacc[2] = getAcclOutput(2);
vector3d_normalize(Kacc);
//calculate correction vector to bring dcmEst's K vector closer to Acc vector (K vector according to accelerometer)
float wA[3];
vector3d_cross(dcmEst[2],Kacc,wA); // wA = Kgyro x Kacc , rotation needed to bring Kacc to Kgyro
//---------------
//dcmEst
//---------------
//gyro rate direction is usually specified (in datasheets) as the device's(body's) rotation
//about a fixed earth's (global) frame, if we look from the perspective of device then
//the global vectors (I,K,J) rotation direction will be the inverse
float w[3]; //gyro rates (angular velocity of a global vector in local coordinates)
w[0] = -getGyroOutput(0); //rotation rate about accelerometer's X axis (GY output) in rad/ms
w[1] = -getGyroOutput(1); //rotation rate about accelerometer's Y axis (GX output) in rad/ms
w[2] = -getGyroOutput(2); //rotation rate about accelerometer's Z axis (GZ output) in rad/ms
for(i=0;i<3;i++){
w[i] *= imu_interval_ms; //scale by elapsed time to get angle in radians
//compute weighted average with the accelerometer correction vector
w[i] = (w[i] + ACC_WEIGHT*wA[i])/(1.0+ACC_WEIGHT);
}
dcm_rotate(dcmEst,w);
}
#endif
double atan2deg(float y, float x){
if(fabs(y) < 0.1 && fabs(x) < 0.1)
return 0.0; //avoid atan2 returning meaningless numbers
else
return atan2(y,x)*180.0 / PI;
}
void euler_angles(float vector[3][3]){
pitch = asin(-vector[2][0]) * 180/PI;
roll = atan2deg(vector[2][1],vector[2][2]); //* 180/PI;
yaw = atan2deg(vector[1][0],vector[0][0]); //* 180/PI;
}
void print_euler(){
uart2_printDouble(pitch, 3);
UART2_Sendchar(',');
uart2_printDouble(roll, 3);
UART2_Sendchar(',');
uart2_printDouble(yaw, 3);
UART2_Sendchar(',');
uart2_printDouble(pitchGyro* 180/PI, 3);
UART2_Sendchar(',');
uart2_printDouble(rollGyro* 180/PI, 3);
UART2_Sendchar(',');
uart2_printDouble(pitchAccel, 3);
UART2_Sendchar(',');
uart2_printDouble(rollAccel, 3);
UART2_PrintString ("\r\n");
}
while ( 1 ){
//Ya pasaron 5mseg por lo que leo los sensores
if(f_leer_data){
L3G4200D_read_data(); //los sensores.
ADXL345_read_data();
f_leer_data=0;
}
if((msTicks-timer) >= 10 ){
timer=msTicks;
imu_update();
}
if((msTicks-timer2) >= 50 ){
timer2=msTicks;
//print_dcm(dcmEst);
euler_angles(dcmEst);
print_euler();
}
}
void imu_update(){
int i;
imu_sequence++;
//interval since last call
imu_interval_ms = 0.00025*(float)(count250us-count250us_prev);
count250us_prev = count250us;
//---------------
// I,J,K unity vectors of global coordinate system I-North,J-West,K-zenith
// i,j,k unity vectors of body's coordiante system i-"nose", j-"left wing", k-"top"
//---------------
// [I.i , I.j, I.k]
// DCM = [J.i , J.j, J.k]
// [K.i , K.j, K.k]
//---------------
//Acelerometer
//---------------
//Accelerometer measures gravity vector G in body coordinate system
//Gravity vector is the reverse of K unity vector of global system expressed in local coordinates
//K vector coincides with the z coordinate of body's i,j,k vectors expressed in global coordinates (K.i , K.j, K.k)
//Acc can estimate global K vector(zenith) measured in body's coordinate systems (the reverse of gravitation vector)
Kacc[0] = getAcclOutput(0);
Kacc[1] = getAcclOutput(1);
Kacc[2] = getAcclOutput(2);
vector3d_normalize(Kacc);
pitchAccel = atan (Kacc[0] / sqrt(Kacc[1]*Kacc[1] + Kacc[2]*Kacc[2])) * 180/PI;
rollAccel = atan (Kacc[1] / sqrt(Kacc[0]*Kacc[0] + Kacc[2]*Kacc[2])) * 180/PI;
//calculate correction vector to bring dcmEst's K vector closer to Acc vector (K vector according to accelerometer)
float wA[3];
vector3d_cross(dcmEst[2],Kacc,wA); // wA = Kgyro x Kacc , rotation needed to bring Kacc to Kgyro
//---------------
//dcmEst
//---------------
//gyro rate direction is usually specified (in datasheets) as the device's(body's) rotation
//about a fixed earth's (global) frame, if we look from the perspective of device then
//the global vectors (I,K,J) rotation direction will be the inverse
float w[3]; //gyro rates (angular velocity of a global vector in local coordinates)
w[0] = -getGyroOutput(0); //rotation rate about accelerometer's X axis (GY output) in rad/ms
w[1] = -getGyroOutput(1); //rotation rate about accelerometer's Y axis (GX output) in rad/ms
w[2] = -getGyroOutput(2); //rotation rate about accelerometer's Z axis (GZ output) in rad/ms
pitchGyro = pitchGyro - w[1] * imu_interval_ms;
rollGyro = rollGyro - w[0] * imu_interval_ms;
for(i=0;i<3;i++){
w[i] *= imu_interval_ms; //scale by elapsed time to get angle in radians
//compute weighted average with the accelerometer correction vector
w[i] = (w[i] + ACC_WEIGHT*wA[i])/(1.0+ACC_WEIGHT);
}
dcm_rotate(dcmEst,w);
}