RazorAHRS - Port of http://dev.qu.tu-berlin.de/projects/sf-ra…

Users » lpetre » Code » RazorAHRS

Luke Petre / Mbed 2 deprecated RazorAHRS

Port of http://dev.qu.tu-berlin.de/projects/sf-razor-9dof-ahrs to an mbed, tested with a 9DOF Sensor Stick, SEN-10724

DCM.cpp@2:5aa75c3d8cc3, 2011-12-28 (annotated)

Committer:: lpetre
Date:: Wed Dec 28 18:09:14 2011 +0000
Revision:: 2:5aa75c3d8cc3
Parent:: 0:9a72d42c0da3

Who changed what in which revision?

User	Revision	Line number	New contents of line
lpetre	0:9a72d42c0da3	1	/* This file is part of the Razor AHRS Firmware */
lpetre	0:9a72d42c0da3	2	#include "Razor_AHRS.h"
lpetre	0:9a72d42c0da3	3	#include <math.h>
lpetre	0:9a72d42c0da3	4
lpetre	0:9a72d42c0da3	5	// DCM algorithm
lpetre	0:9a72d42c0da3	6
lpetre	0:9a72d42c0da3	7	/**************************************************/
lpetre	0:9a72d42c0da3	8	void IMU::Normalize(void)
lpetre	0:9a72d42c0da3	9	{
lpetre	0:9a72d42c0da3	10	float error=0;
lpetre	0:9a72d42c0da3	11	float temporary[3][3];
lpetre	0:9a72d42c0da3	12	float renorm=0;
lpetre	0:9a72d42c0da3	13
lpetre	0:9a72d42c0da3	14	error= -Vector_Dot_Product(&DCM_Matrix[0][0],&DCM_Matrix[1][0])*.5; //eq.19
lpetre	0:9a72d42c0da3	15
lpetre	0:9a72d42c0da3	16	Vector_Scale(&temporary[0][0], &DCM_Matrix[1][0], error); //eq.19
lpetre	0:9a72d42c0da3	17	Vector_Scale(&temporary[1][0], &DCM_Matrix[0][0], error); //eq.19
lpetre	0:9a72d42c0da3	18
lpetre	0:9a72d42c0da3	19	Vector_Add(&temporary[0][0], &temporary[0][0], &DCM_Matrix[0][0]);//eq.19
lpetre	0:9a72d42c0da3	20	Vector_Add(&temporary[1][0], &temporary[1][0], &DCM_Matrix[1][0]);//eq.19
lpetre	0:9a72d42c0da3	21
lpetre	0:9a72d42c0da3	22	Vector_Cross_Product(&temporary[2][0],&temporary[0][0],&temporary[1][0]); // c= a x b //eq.20
lpetre	0:9a72d42c0da3	23
lpetre	0:9a72d42c0da3	24	renorm= .5 *(3 - Vector_Dot_Product(&temporary[0][0],&temporary[0][0])); //eq.21
lpetre	0:9a72d42c0da3	25	Vector_Scale(&DCM_Matrix[0][0], &temporary[0][0], renorm);
lpetre	0:9a72d42c0da3	26
lpetre	0:9a72d42c0da3	27	renorm= .5 *(3 - Vector_Dot_Product(&temporary[1][0],&temporary[1][0])); //eq.21
lpetre	0:9a72d42c0da3	28	Vector_Scale(&DCM_Matrix[1][0], &temporary[1][0], renorm);
lpetre	0:9a72d42c0da3	29
lpetre	0:9a72d42c0da3	30	renorm= .5 *(3 - Vector_Dot_Product(&temporary[2][0],&temporary[2][0])); //eq.21
lpetre	0:9a72d42c0da3	31	Vector_Scale(&DCM_Matrix[2][0], &temporary[2][0], renorm);
lpetre	0:9a72d42c0da3	32	}
lpetre	0:9a72d42c0da3	33
lpetre	0:9a72d42c0da3	34	/**************************************************/
lpetre	0:9a72d42c0da3	35	void IMU::Drift_correction(void)
lpetre	0:9a72d42c0da3	36	{
lpetre	0:9a72d42c0da3	37	float mag_heading_x;
lpetre	0:9a72d42c0da3	38	float mag_heading_y;
lpetre	0:9a72d42c0da3	39	float errorCourse;
lpetre	0:9a72d42c0da3	40	//Compensation the Roll, Pitch and Yaw drift.
lpetre	0:9a72d42c0da3	41	static float Scaled_Omega_P[3];
lpetre	0:9a72d42c0da3	42	static float Scaled_Omega_I[3];
lpetre	0:9a72d42c0da3	43	float Accel_magnitude;
lpetre	0:9a72d42c0da3	44	float Accel_weight;
lpetre	0:9a72d42c0da3	45
lpetre	0:9a72d42c0da3	46
lpetre	0:9a72d42c0da3	47	//***Roll and Pitch*************
lpetre	0:9a72d42c0da3	48
lpetre	0:9a72d42c0da3	49	// Calculate the magnitude of the accelerometer vector
lpetre	0:9a72d42c0da3	50	Accel_magnitude = sqrt(Accel_Vector[0]Accel_Vector[0] + Accel_Vector[1]Accel_Vector[1] + Accel_Vector[2]*Accel_Vector[2]);
lpetre	0:9a72d42c0da3	51	Accel_magnitude = Accel_magnitude / GRAVITY; // Scale to gravity.
lpetre	0:9a72d42c0da3	52	// Dynamic weighting of accelerometer info (reliability filter)
lpetre	0:9a72d42c0da3	53	// Weight for accelerometer info (<0.5G = 0.0, 1G = 1.0 , >1.5G = 0.0)
lpetre	0:9a72d42c0da3	54	Accel_weight = constrain(1 - 2*abs(1 - Accel_magnitude),0,1); //
lpetre	0:9a72d42c0da3	55
lpetre	0:9a72d42c0da3	56	Vector_Cross_Product(&errorRollPitch[0],&Accel_Vector[0],&DCM_Matrix[2][0]); //adjust the ground of reference
lpetre	0:9a72d42c0da3	57	Vector_Scale(&Omega_P[0],&errorRollPitch[0],Kp_ROLLPITCH*Accel_weight);
lpetre	0:9a72d42c0da3	58
lpetre	0:9a72d42c0da3	59	Vector_Scale(&Scaled_Omega_I[0],&errorRollPitch[0],Ki_ROLLPITCH*Accel_weight);
lpetre	0:9a72d42c0da3	60	Vector_Add(Omega_I,Omega_I,Scaled_Omega_I);
lpetre	0:9a72d42c0da3	61
lpetre	0:9a72d42c0da3	62	//***YAW*************
lpetre	0:9a72d42c0da3	63	// We make the gyro YAW drift correction based on compass magnetic heading
lpetre	0:9a72d42c0da3	64
lpetre	0:9a72d42c0da3	65	mag_heading_x = cos(MAG_Heading);
lpetre	0:9a72d42c0da3	66	mag_heading_y = sin(MAG_Heading);
lpetre	0:9a72d42c0da3	67	errorCourse=(DCM_Matrix[0][0]mag_heading_y) - (DCM_Matrix[1][0]mag_heading_x); //Calculating YAW error
lpetre	0:9a72d42c0da3	68	Vector_Scale(errorYaw,&DCM_Matrix[2][0],errorCourse); //Applys the yaw correction to the XYZ rotation of the aircraft, depeding the position.
lpetre	0:9a72d42c0da3	69
lpetre	0:9a72d42c0da3	70	Vector_Scale(&Scaled_Omega_P[0],&errorYaw[0],Kp_YAW);//.01proportional of YAW.
lpetre	0:9a72d42c0da3	71	Vector_Add(Omega_P,Omega_P,Scaled_Omega_P);//Adding Proportional.
lpetre	0:9a72d42c0da3	72
lpetre	0:9a72d42c0da3	73	Vector_Scale(&Scaled_Omega_I[0],&errorYaw[0],Ki_YAW);//.00001Integrator
lpetre	0:9a72d42c0da3	74	Vector_Add(Omega_I,Omega_I,Scaled_Omega_I);//adding integrator to the Omega_I
lpetre	0:9a72d42c0da3	75	}
lpetre	0:9a72d42c0da3	76
lpetre	0:9a72d42c0da3	77	void IMU::Matrix_update(void)
lpetre	0:9a72d42c0da3	78	{
lpetre	0:9a72d42c0da3	79	Gyro_Vector[0]=GYRO_SCALED_RAD(gyro[0]); //gyro x roll
lpetre	0:9a72d42c0da3	80	Gyro_Vector[1]=GYRO_SCALED_RAD(gyro[1]); //gyro y pitch
lpetre	0:9a72d42c0da3	81	Gyro_Vector[2]=GYRO_SCALED_RAD(gyro[2]); //gyro z yaw
lpetre	0:9a72d42c0da3	82
lpetre	0:9a72d42c0da3	83	Accel_Vector[0]=accel[0];
lpetre	0:9a72d42c0da3	84	Accel_Vector[1]=accel[1];
lpetre	0:9a72d42c0da3	85	Accel_Vector[2]=accel[2];
lpetre	0:9a72d42c0da3	86
lpetre	0:9a72d42c0da3	87	Vector_Add(&Omega[0], &Gyro_Vector[0], &Omega_I[0]); //adding proportional term
lpetre	0:9a72d42c0da3	88	Vector_Add(&Omega_Vector[0], &Omega[0], &Omega_P[0]); //adding Integrator term
lpetre	0:9a72d42c0da3	89
lpetre	0:9a72d42c0da3	90	#if DEBUG__NO_DRIFT_CORRECTION == true // Do not use drift correction
lpetre	0:9a72d42c0da3	91	Update_Matrix[0][0]=0;
lpetre	0:9a72d42c0da3	92	Update_Matrix[0][1]=-G_Dt*Gyro_Vector[2];//-z
lpetre	0:9a72d42c0da3	93	Update_Matrix[0][2]=G_Dt*Gyro_Vector[1];//y
lpetre	0:9a72d42c0da3	94	Update_Matrix[1][0]=G_Dt*Gyro_Vector[2];//z
lpetre	0:9a72d42c0da3	95	Update_Matrix[1][1]=0;
lpetre	0:9a72d42c0da3	96	Update_Matrix[1][2]=-G_Dt*Gyro_Vector[0];
lpetre	0:9a72d42c0da3	97	Update_Matrix[2][0]=-G_Dt*Gyro_Vector[1];
lpetre	0:9a72d42c0da3	98	Update_Matrix[2][1]=G_Dt*Gyro_Vector[0];
lpetre	0:9a72d42c0da3	99	Update_Matrix[2][2]=0;
lpetre	0:9a72d42c0da3	100	#else // Use drift correction
lpetre	0:9a72d42c0da3	101	Update_Matrix[0][0]=0;
lpetre	0:9a72d42c0da3	102	Update_Matrix[0][1]=-G_Dt*Omega_Vector[2];//-z
lpetre	0:9a72d42c0da3	103	Update_Matrix[0][2]=G_Dt*Omega_Vector[1];//y
lpetre	0:9a72d42c0da3	104	Update_Matrix[1][0]=G_Dt*Omega_Vector[2];//z
lpetre	0:9a72d42c0da3	105	Update_Matrix[1][1]=0;
lpetre	0:9a72d42c0da3	106	Update_Matrix[1][2]=-G_Dt*Omega_Vector[0];//-x
lpetre	0:9a72d42c0da3	107	Update_Matrix[2][0]=-G_Dt*Omega_Vector[1];//-y
lpetre	0:9a72d42c0da3	108	Update_Matrix[2][1]=G_Dt*Omega_Vector[0];//x
lpetre	0:9a72d42c0da3	109	Update_Matrix[2][2]=0;
lpetre	0:9a72d42c0da3	110	#endif
lpetre	0:9a72d42c0da3	111
lpetre	0:9a72d42c0da3	112	Matrix_Multiply(DCM_Matrix,Update_Matrix,Temporary_Matrix); //a*b=c
lpetre	0:9a72d42c0da3	113
lpetre	0:9a72d42c0da3	114	for(int x=0; x<3; x++) //Matrix Addition (update)
lpetre	0:9a72d42c0da3	115	{
lpetre	0:9a72d42c0da3	116	for(int y=0; y<3; y++)
lpetre	0:9a72d42c0da3	117	{
lpetre	0:9a72d42c0da3	118	DCM_Matrix[x][y]+=Temporary_Matrix[x][y];
lpetre	0:9a72d42c0da3	119	}
lpetre	0:9a72d42c0da3	120	}
lpetre	0:9a72d42c0da3	121	}
lpetre	0:9a72d42c0da3	122
lpetre	0:9a72d42c0da3	123	void IMU::Euler_angles(void)
lpetre	0:9a72d42c0da3	124	{
lpetre	0:9a72d42c0da3	125	pitch = -asin(DCM_Matrix[2][0]);
lpetre	0:9a72d42c0da3	126	roll = atan2(DCM_Matrix[2][1],DCM_Matrix[2][2]);
lpetre	0:9a72d42c0da3	127	yaw = atan2(DCM_Matrix[1][0],DCM_Matrix[0][0]);
lpetre	0:9a72d42c0da3	128	}
lpetre	0:9a72d42c0da3	129

Repository toolbox

Export to desktop IDE

Build repository

Repository details

Type:	Program
Mbed OS support:	Mbed 2 deprecated
Created:	27 Dec 2011
Imports:	212
Forks:	1
Commits:	3
Dependents:	0
Dependencies:	2
Followers:	5

DCM.cpp@2:5aa75c3d8cc3, 2011-12-28 (annotated)

Who changed what in which revision?

Repository toolbox

Repository details

Important Information for this Arm website

Access Warning