cmsis_rtos_IMU_test1 - Test1 of cmsis and IMU/AHRS (sensor BMA180,HMC588…

Users » caroe » Code » cmsis_rtos_IMU_test1

Bo Carøe / Mbed 2 deprecated cmsis_rtos_IMU_test1

Test1 of cmsis and IMU/AHRS (sensor BMA180,HMC5883,ITG3200) IMU/AHRS is not ok

IMUfilter/IMUfilter.cpp@0:cb04b53e6f9b, 2012-06-11 (annotated)

Committer:: caroe
Date:: Mon Jun 11 12:02:30 2012 +0000
Revision:: 0:cb04b53e6f9b

Who changed what in which revision?

User	Revision	Line number	New contents of line
caroe	0:cb04b53e6f9b	1	/**
caroe	0:cb04b53e6f9b	2	* @author Aaron Berk
caroe	0:cb04b53e6f9b	3	*
caroe	0:cb04b53e6f9b	4	* @section LICENSE
caroe	0:cb04b53e6f9b	5	*
caroe	0:cb04b53e6f9b	6	* Copyright (c) 2010 ARM Limited
caroe	0:cb04b53e6f9b	7	*
caroe	0:cb04b53e6f9b	8	* Permission is hereby granted, free of charge, to any person obtaining a copy
caroe	0:cb04b53e6f9b	9	* of this software and associated documentation files (the "Software"), to deal
caroe	0:cb04b53e6f9b	10	* in the Software without restriction, including without limitation the rights
caroe	0:cb04b53e6f9b	11	* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
caroe	0:cb04b53e6f9b	12	* copies of the Software, and to permit persons to whom the Software is
caroe	0:cb04b53e6f9b	13	* furnished to do so, subject to the following conditions:
caroe	0:cb04b53e6f9b	14	*
caroe	0:cb04b53e6f9b	15	* The above copyright notice and this permission notice shall be included in
caroe	0:cb04b53e6f9b	16	* all copies or substantial portions of the Software.
caroe	0:cb04b53e6f9b	17	*
caroe	0:cb04b53e6f9b	18	* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
caroe	0:cb04b53e6f9b	19	* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
caroe	0:cb04b53e6f9b	20	* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
caroe	0:cb04b53e6f9b	21	* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
caroe	0:cb04b53e6f9b	22	* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
caroe	0:cb04b53e6f9b	23	* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
caroe	0:cb04b53e6f9b	24	* THE SOFTWARE.
caroe	0:cb04b53e6f9b	25	*
caroe	0:cb04b53e6f9b	26	* @section DESCRIPTION
caroe	0:cb04b53e6f9b	27	*
caroe	0:cb04b53e6f9b	28	* IMU orientation filter developed by Sebastian Madgwick.
caroe	0:cb04b53e6f9b	29	*
caroe	0:cb04b53e6f9b	30	* Find more details about his paper here:
caroe	0:cb04b53e6f9b	31	*
caroe	0:cb04b53e6f9b	32	* http://code.google.com/p/imumargalgorithm30042010sohm/
caroe	0:cb04b53e6f9b	33	*/
caroe	0:cb04b53e6f9b	34
caroe	0:cb04b53e6f9b	35	/**
caroe	0:cb04b53e6f9b	36	* Includes
caroe	0:cb04b53e6f9b	37	*/
caroe	0:cb04b53e6f9b	38	#include "IMUfilter.h"
caroe	0:cb04b53e6f9b	39
caroe	0:cb04b53e6f9b	40	IMUfilter::IMUfilter(Timer & GlobalTime): GlobalTime(GlobalTime){
caroe	0:cb04b53e6f9b	41
caroe	0:cb04b53e6f9b	42	firstUpdate = 0;
caroe	0:cb04b53e6f9b	43
caroe	0:cb04b53e6f9b	44	//Quaternion orientation of earth frame relative to auxiliary frame.
caroe	0:cb04b53e6f9b	45	AEq_1 = 1;
caroe	0:cb04b53e6f9b	46	AEq_2 = 0;
caroe	0:cb04b53e6f9b	47	AEq_3 = 0;
caroe	0:cb04b53e6f9b	48	AEq_4 = 0;
caroe	0:cb04b53e6f9b	49
caroe	0:cb04b53e6f9b	50	//Estimated orientation quaternion elements with initial conditions.
caroe	0:cb04b53e6f9b	51	SEq_1 = 1;
caroe	0:cb04b53e6f9b	52	SEq_2 = 0;
caroe	0:cb04b53e6f9b	53	SEq_3 = 0;
caroe	0:cb04b53e6f9b	54	SEq_4 = 0;
caroe	0:cb04b53e6f9b	55
caroe	0:cb04b53e6f9b	56	//Sampling period (typical value is ~0.1s).
caroe	0:cb04b53e6f9b	57	deltat =0.1;
caroe	0:cb04b53e6f9b	58
caroe	0:cb04b53e6f9b	59	//Gyroscope measurement error (in degrees per second).
caroe	0:cb04b53e6f9b	60	gyroMeasError = 0;
caroe	0:cb04b53e6f9b	61
caroe	0:cb04b53e6f9b	62	//Compute beta.
caroe	0:cb04b53e6f9b	63	beta = sqrt(3.0 / 4.0) * (PI * (gyroMeasError / 180.0));
caroe	0:cb04b53e6f9b	64
caroe	0:cb04b53e6f9b	65	}
caroe	0:cb04b53e6f9b	66
caroe	0:cb04b53e6f9b	67	void IMUfilter::updateFilter(double w_x, double w_y, double w_z, double a_x, double a_y, double a_z) {
caroe	0:cb04b53e6f9b	68
caroe	0:cb04b53e6f9b	69	//Local system variables.
caroe	0:cb04b53e6f9b	70
caroe	0:cb04b53e6f9b	71	//Vector norm.
caroe	0:cb04b53e6f9b	72	double norm;
caroe	0:cb04b53e6f9b	73	//Quaternion rate from gyroscope elements.
caroe	0:cb04b53e6f9b	74	double SEqDot_omega_1;
caroe	0:cb04b53e6f9b	75	double SEqDot_omega_2;
caroe	0:cb04b53e6f9b	76	double SEqDot_omega_3;
caroe	0:cb04b53e6f9b	77	double SEqDot_omega_4;
caroe	0:cb04b53e6f9b	78	//Objective function elements.
caroe	0:cb04b53e6f9b	79	double f_1;
caroe	0:cb04b53e6f9b	80	double f_2;
caroe	0:cb04b53e6f9b	81	double f_3;
caroe	0:cb04b53e6f9b	82	//Objective function Jacobian elements.
caroe	0:cb04b53e6f9b	83	double J_11or24;
caroe	0:cb04b53e6f9b	84	double J_12or23;
caroe	0:cb04b53e6f9b	85	double J_13or22;
caroe	0:cb04b53e6f9b	86	double J_14or21;
caroe	0:cb04b53e6f9b	87	double J_32;
caroe	0:cb04b53e6f9b	88	double J_33;
caroe	0:cb04b53e6f9b	89	//Objective function gradient elements.
caroe	0:cb04b53e6f9b	90	double nablaf_1;
caroe	0:cb04b53e6f9b	91	double nablaf_2;
caroe	0:cb04b53e6f9b	92	double nablaf_3;
caroe	0:cb04b53e6f9b	93	double nablaf_4;
caroe	0:cb04b53e6f9b	94
caroe	0:cb04b53e6f9b	95	//Auxiliary variables to avoid reapeated calculations.
caroe	0:cb04b53e6f9b	96	double halfSEq_1 = 0.5 * SEq_1;
caroe	0:cb04b53e6f9b	97	double halfSEq_2 = 0.5 * SEq_2;
caroe	0:cb04b53e6f9b	98	double halfSEq_3 = 0.5 * SEq_3;
caroe	0:cb04b53e6f9b	99	double halfSEq_4 = 0.5 * SEq_4;
caroe	0:cb04b53e6f9b	100	double twoSEq_1 = 2.0 * SEq_1;
caroe	0:cb04b53e6f9b	101	double twoSEq_2 = 2.0 * SEq_2;
caroe	0:cb04b53e6f9b	102	double twoSEq_3 = 2.0 * SEq_3;
caroe	0:cb04b53e6f9b	103
caroe	0:cb04b53e6f9b	104	//Time Step (=Zeitdifferenz berechnen)
caroe	0:cb04b53e6f9b	105	static int LastFrame= GlobalTime.read_us();
caroe	0:cb04b53e6f9b	106	int Now= GlobalTime.read_us();
caroe	0:cb04b53e6f9b	107	deltat= 0.000001 * (float)(Now - LastFrame);
caroe	0:cb04b53e6f9b	108	LastFrame= Now;
caroe	0:cb04b53e6f9b	109
caroe	0:cb04b53e6f9b	110	//Compute the quaternion rate measured by gyroscopes.
caroe	0:cb04b53e6f9b	111	SEqDot_omega_1 = -halfSEq_2 * w_x - halfSEq_3 * w_y - halfSEq_4 * w_z;
caroe	0:cb04b53e6f9b	112	SEqDot_omega_2 = halfSEq_1 * w_x + halfSEq_3 * w_z - halfSEq_4 * w_y;
caroe	0:cb04b53e6f9b	113	SEqDot_omega_3 = halfSEq_1 * w_y - halfSEq_2 * w_z + halfSEq_4 * w_x;
caroe	0:cb04b53e6f9b	114	SEqDot_omega_4 = halfSEq_1 * w_z + halfSEq_2 * w_y - halfSEq_3 * w_x;
caroe	0:cb04b53e6f9b	115
caroe	0:cb04b53e6f9b	116	//Normalise the accelerometer measurement.
caroe	0:cb04b53e6f9b	117	norm = sqrt(a_x * a_x + a_y * a_y + a_z * a_z);
caroe	0:cb04b53e6f9b	118	a_x /= norm;
caroe	0:cb04b53e6f9b	119	a_y /= norm;
caroe	0:cb04b53e6f9b	120	a_z /= norm;
caroe	0:cb04b53e6f9b	121
caroe	0:cb04b53e6f9b	122	//Compute the objective function and Jacobian.
caroe	0:cb04b53e6f9b	123	f_1 = twoSEq_2 * SEq_4 - twoSEq_1 * SEq_3 - a_x;
caroe	0:cb04b53e6f9b	124	f_2 = twoSEq_1 * SEq_2 + twoSEq_3 * SEq_4 - a_y;
caroe	0:cb04b53e6f9b	125	f_3 = 1.0 - twoSEq_2 * SEq_2 - twoSEq_3 * SEq_3 - a_z;
caroe	0:cb04b53e6f9b	126	//J_11 negated in matrix multiplication.
caroe	0:cb04b53e6f9b	127	J_11or24 = twoSEq_3;
caroe	0:cb04b53e6f9b	128	J_12or23 = 2 * SEq_4;
caroe	0:cb04b53e6f9b	129	//J_12 negated in matrix multiplication
caroe	0:cb04b53e6f9b	130	J_13or22 = twoSEq_1;
caroe	0:cb04b53e6f9b	131	J_14or21 = twoSEq_2;
caroe	0:cb04b53e6f9b	132	//Negated in matrix multiplication.
caroe	0:cb04b53e6f9b	133	J_32 = 2 * J_14or21;
caroe	0:cb04b53e6f9b	134	//Negated in matrix multiplication.
caroe	0:cb04b53e6f9b	135	J_33 = 2 * J_11or24;
caroe	0:cb04b53e6f9b	136
caroe	0:cb04b53e6f9b	137	//Compute the gradient (matrix multiplication).
caroe	0:cb04b53e6f9b	138	nablaf_1 = J_14or21 * f_2 - J_11or24 * f_1;
caroe	0:cb04b53e6f9b	139	nablaf_2 = J_12or23 * f_1 + J_13or22 * f_2 - J_32 * f_3;
caroe	0:cb04b53e6f9b	140	nablaf_3 = J_12or23 * f_2 - J_33 * f_3 - J_13or22 * f_1;
caroe	0:cb04b53e6f9b	141	nablaf_4 = J_14or21 * f_1 + J_11or24 * f_2;
caroe	0:cb04b53e6f9b	142
caroe	0:cb04b53e6f9b	143	//Normalise the gradient.
caroe	0:cb04b53e6f9b	144	norm = sqrt(nablaf_1 * nablaf_1 + nablaf_2 * nablaf_2 + nablaf_3 * nablaf_3 + nablaf_4 * nablaf_4);
caroe	0:cb04b53e6f9b	145	nablaf_1 /= norm;
caroe	0:cb04b53e6f9b	146	nablaf_2 /= norm;
caroe	0:cb04b53e6f9b	147	nablaf_3 /= norm;
caroe	0:cb04b53e6f9b	148	nablaf_4 /= norm;
caroe	0:cb04b53e6f9b	149
caroe	0:cb04b53e6f9b	150	//Compute then integrate the estimated quaternion rate.
caroe	0:cb04b53e6f9b	151	SEq_1 += (SEqDot_omega_1 - (beta * nablaf_1)) * deltat;
caroe	0:cb04b53e6f9b	152	SEq_2 += (SEqDot_omega_2 - (beta * nablaf_2)) * deltat;
caroe	0:cb04b53e6f9b	153	SEq_3 += (SEqDot_omega_3 - (beta * nablaf_3)) * deltat;
caroe	0:cb04b53e6f9b	154	SEq_4 += (SEqDot_omega_4 - (beta * nablaf_4)) * deltat;
caroe	0:cb04b53e6f9b	155
caroe	0:cb04b53e6f9b	156	//Normalise quaternion
caroe	0:cb04b53e6f9b	157	norm = sqrt(SEq_1 * SEq_1 + SEq_2 * SEq_2 + SEq_3 * SEq_3 + SEq_4 * SEq_4);
caroe	0:cb04b53e6f9b	158	SEq_1 /= norm;
caroe	0:cb04b53e6f9b	159	SEq_2 /= norm;
caroe	0:cb04b53e6f9b	160	SEq_3 /= norm;
caroe	0:cb04b53e6f9b	161	SEq_4 /= norm;
caroe	0:cb04b53e6f9b	162
caroe	0:cb04b53e6f9b	163	if (firstUpdate == 0) {
caroe	0:cb04b53e6f9b	164	//Store orientation of auxiliary frame.
caroe	0:cb04b53e6f9b	165	AEq_1 = SEq_1;
caroe	0:cb04b53e6f9b	166	AEq_2 = SEq_2;
caroe	0:cb04b53e6f9b	167	AEq_3 = SEq_3;
caroe	0:cb04b53e6f9b	168	AEq_4 = SEq_4;
caroe	0:cb04b53e6f9b	169	firstUpdate = 1;
caroe	0:cb04b53e6f9b	170	}
caroe	0:cb04b53e6f9b	171
caroe	0:cb04b53e6f9b	172	}
caroe	0:cb04b53e6f9b	173
caroe	0:cb04b53e6f9b	174	void IMUfilter::computeEuler(void){
caroe	0:cb04b53e6f9b	175
caroe	0:cb04b53e6f9b	176	//Quaternion describing orientation of sensor relative to earth.
caroe	0:cb04b53e6f9b	177	double ESq_1, ESq_2, ESq_3, ESq_4;
caroe	0:cb04b53e6f9b	178	//Quaternion describing orientation of sensor relative to auxiliary frame.
caroe	0:cb04b53e6f9b	179	double ASq_1, ASq_2, ASq_3, ASq_4;
caroe	0:cb04b53e6f9b	180
caroe	0:cb04b53e6f9b	181	//Compute the quaternion conjugate.
caroe	0:cb04b53e6f9b	182	ESq_1 = SEq_1;
caroe	0:cb04b53e6f9b	183	ESq_2 = -SEq_2;
caroe	0:cb04b53e6f9b	184	ESq_3 = -SEq_3;
caroe	0:cb04b53e6f9b	185	ESq_4 = -SEq_4;
caroe	0:cb04b53e6f9b	186
caroe	0:cb04b53e6f9b	187	//Compute the quaternion product.
caroe	0:cb04b53e6f9b	188	ASq_1 = ESq_1 * AEq_1 - ESq_2 * AEq_2 - ESq_3 * AEq_3 - ESq_4 * AEq_4;
caroe	0:cb04b53e6f9b	189	ASq_2 = ESq_1 * AEq_2 + ESq_2 * AEq_1 + ESq_3 * AEq_4 - ESq_4 * AEq_3;
caroe	0:cb04b53e6f9b	190	ASq_3 = ESq_1 * AEq_3 - ESq_2 * AEq_4 + ESq_3 * AEq_1 + ESq_4 * AEq_2;
caroe	0:cb04b53e6f9b	191	ASq_4 = ESq_1 * AEq_4 + ESq_2 * AEq_3 - ESq_3 * AEq_2 + ESq_4 * AEq_1;
caroe	0:cb04b53e6f9b	192
caroe	0:cb04b53e6f9b	193	//Compute the Euler angles from the quaternion.
caroe	0:cb04b53e6f9b	194	phi = atan2(2 * ASq_3 * ASq_4 - 2 * ASq_1 * ASq_2, 2 * ASq_1 * ASq_1 + 2 * ASq_4 * ASq_4 - 1);
caroe	0:cb04b53e6f9b	195	theta = asin (2 * ASq_2 * ASq_4 - 2 * ASq_1 * ASq_3);
caroe	0:cb04b53e6f9b	196	psi = atan2(2 * ASq_2 * ASq_3 - 2 * ASq_1 * ASq_4, 2 * ASq_1 * ASq_1 + 2 * ASq_2 * ASq_2 - 1);
caroe	0:cb04b53e6f9b	197	//asin(2 * ASq_2 * ASq_3 - 2 * ASq_1 * ASq_3); With theta = asin(2 * ASq_2 * ASq_4 - 2 * ASq_1 * ASq_3);
caroe	0:cb04b53e6f9b	198	}
caroe	0:cb04b53e6f9b	199
caroe	0:cb04b53e6f9b	200	double IMUfilter::getRoll(void){
caroe	0:cb04b53e6f9b	201
caroe	0:cb04b53e6f9b	202	return phi;
caroe	0:cb04b53e6f9b	203
caroe	0:cb04b53e6f9b	204	}
caroe	0:cb04b53e6f9b	205
caroe	0:cb04b53e6f9b	206	double IMUfilter::getPitch(void){
caroe	0:cb04b53e6f9b	207
caroe	0:cb04b53e6f9b	208	return theta;
caroe	0:cb04b53e6f9b	209
caroe	0:cb04b53e6f9b	210	}
caroe	0:cb04b53e6f9b	211
caroe	0:cb04b53e6f9b	212	double IMUfilter::getYaw(void){
caroe	0:cb04b53e6f9b	213
caroe	0:cb04b53e6f9b	214	return psi;
caroe	0:cb04b53e6f9b	215
caroe	0:cb04b53e6f9b	216	}
caroe	0:cb04b53e6f9b	217
caroe	0:cb04b53e6f9b	218	void IMUfilter::setGyroError(double gyroscopeMeasurementError)
caroe	0:cb04b53e6f9b	219	{
caroe	0:cb04b53e6f9b	220	gyroMeasError = gyroscopeMeasurementError;
caroe	0:cb04b53e6f9b	221	beta = sqrt(3.0 / 4.0) * (PI * (gyroMeasError / 180.0));
caroe	0:cb04b53e6f9b	222	// reset();
caroe	0:cb04b53e6f9b	223	}
caroe	0:cb04b53e6f9b	224
caroe	0:cb04b53e6f9b	225	double IMUfilter::getGyroError(void)
caroe	0:cb04b53e6f9b	226	{
caroe	0:cb04b53e6f9b	227	return gyroMeasError;
caroe	0:cb04b53e6f9b	228	}
caroe	0:cb04b53e6f9b	229
caroe	0:cb04b53e6f9b	230	void IMUfilter::reset(void) {
caroe	0:cb04b53e6f9b	231
caroe	0:cb04b53e6f9b	232	firstUpdate = 0;
caroe	0:cb04b53e6f9b	233
caroe	0:cb04b53e6f9b	234	//Quaternion orientation of earth frame relative to auxiliary frame.
caroe	0:cb04b53e6f9b	235	AEq_1 = 1;
caroe	0:cb04b53e6f9b	236	AEq_2 = 0;
caroe	0:cb04b53e6f9b	237	AEq_3 = 0;
caroe	0:cb04b53e6f9b	238	AEq_4 = 0;
caroe	0:cb04b53e6f9b	239
caroe	0:cb04b53e6f9b	240	//Estimated orientation quaternion elements with initial conditions.
caroe	0:cb04b53e6f9b	241	SEq_1 = 1;
caroe	0:cb04b53e6f9b	242	SEq_2 = 0;
caroe	0:cb04b53e6f9b	243	SEq_3 = 0;
caroe	0:cb04b53e6f9b	244	SEq_4 = 0;
caroe	0:cb04b53e6f9b	245
caroe	0:cb04b53e6f9b	246	//Compute beta.
caroe	0:cb04b53e6f9b	247	beta = sqrt(3.0 / 4.0) * (PI * (gyroMeasError / 180.0));
caroe	0:cb04b53e6f9b	248	}

Repository toolbox

Export to desktop IDE

Build repository

Repository details

Type:	Program
Mbed OS support:	Mbed 2 deprecated
Created:	11 Jun 2012
Imports:	247
Forks:	0
Commits:	1
Dependents:	0
Dependencies:	1
Followers:	6

IMUfilter/IMUfilter.cpp@0:cb04b53e6f9b, 2012-06-11 (annotated)

Who changed what in which revision?

Repository toolbox

Repository details

Important Information for this Arm website

Access Warning