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

Razor_AHRS.cpp@0:9a72d42c0da3, 2011-12-27 (annotated)

Committer:: lpetre
Date:: Tue Dec 27 17:20:06 2011 +0000
Revision:: 0:9a72d42c0da3
Child:: 2:5aa75c3d8cc3

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	void IMU::read_sensors() {
lpetre	0:9a72d42c0da3	6	Read_Gyro(); // Read gyroscope
lpetre	0:9a72d42c0da3	7	Read_Accel(); // Read accelerometer
lpetre	0:9a72d42c0da3	8	Read_Magn(); // Read magnetometer
lpetre	0:9a72d42c0da3	9	}
lpetre	0:9a72d42c0da3	10
lpetre	0:9a72d42c0da3	11	// Read every sensor and record a time stamp
lpetre	0:9a72d42c0da3	12	// Init DCM with unfiltered orientation
lpetre	0:9a72d42c0da3	13	// TODO re-init global vars?
lpetre	0:9a72d42c0da3	14	void IMU::reset_sensor_fusion() {
lpetre	0:9a72d42c0da3	15	float temp0[3] = { accel[0], accel[1], accel[2] };
lpetre	0:9a72d42c0da3	16	float temp1[3];
lpetre	0:9a72d42c0da3	17	float temp2[3];
lpetre	0:9a72d42c0da3	18	float xAxis[] = {1.0f, 0.0f, 0.0f};
lpetre	0:9a72d42c0da3	19
lpetre	0:9a72d42c0da3	20	read_sensors();
lpetre	0:9a72d42c0da3	21	timestamp = timer.read_ms();
lpetre	0:9a72d42c0da3	22
lpetre	0:9a72d42c0da3	23	// GET PITCH
lpetre	0:9a72d42c0da3	24	// Using y-z-plane-component/x-component of gravity vector
lpetre	0:9a72d42c0da3	25	pitch = -atan2(accel[0], sqrt((double)(accel[1] * accel[1] + accel[2] * accel[2])));
lpetre	0:9a72d42c0da3	26
lpetre	0:9a72d42c0da3	27	// GET ROLL
lpetre	0:9a72d42c0da3	28	// Compensate pitch of gravity vector
lpetre	0:9a72d42c0da3	29
lpetre	0:9a72d42c0da3	30	Vector_Cross_Product(temp1, temp0, xAxis);
lpetre	0:9a72d42c0da3	31	Vector_Cross_Product(temp2, xAxis, temp1);
lpetre	0:9a72d42c0da3	32	// Normally using x-z-plane-component/y-component of compensated gravity vector
lpetre	0:9a72d42c0da3	33	// roll = atan2(temp2[1], sqrt(temp2[0] * temp2[0] + temp2[2] * temp2[2]));
lpetre	0:9a72d42c0da3	34	// Since we compensated for pitch, x-z-plane-component equals z-component:
lpetre	0:9a72d42c0da3	35	roll = atan2(temp2[1], temp2[2]);
lpetre	0:9a72d42c0da3	36
lpetre	0:9a72d42c0da3	37	// GET YAW
lpetre	0:9a72d42c0da3	38	Compass_Heading();
lpetre	0:9a72d42c0da3	39	yaw = MAG_Heading;
lpetre	0:9a72d42c0da3	40
lpetre	0:9a72d42c0da3	41	// Init rotation matrix
lpetre	0:9a72d42c0da3	42	init_rotation_matrix(DCM_Matrix, yaw, pitch, roll);
lpetre	0:9a72d42c0da3	43	}
lpetre	0:9a72d42c0da3	44
lpetre	0:9a72d42c0da3	45	// Apply calibration to raw sensor readings
lpetre	0:9a72d42c0da3	46	void IMU::compensate_sensor_errors() {
lpetre	0:9a72d42c0da3	47	// Compensate accelerometer error
lpetre	0:9a72d42c0da3	48	accel[0] = (accel[0] - ACCEL_X_OFFSET) * ACCEL_X_SCALE;
lpetre	0:9a72d42c0da3	49	accel[1] = (accel[1] - ACCEL_Y_OFFSET) * ACCEL_Y_SCALE;
lpetre	0:9a72d42c0da3	50	accel[2] = (accel[2] - ACCEL_Z_OFFSET) * ACCEL_Z_SCALE;
lpetre	0:9a72d42c0da3	51
lpetre	0:9a72d42c0da3	52	// Compensate magnetometer error
lpetre	0:9a72d42c0da3	53	magnetom[0] = (magnetom[0] - MAGN_X_OFFSET) * MAGN_X_SCALE;
lpetre	0:9a72d42c0da3	54	magnetom[1] = (magnetom[1] - MAGN_Y_OFFSET) * MAGN_Y_SCALE;
lpetre	0:9a72d42c0da3	55	magnetom[2] = (magnetom[2] - MAGN_Z_OFFSET) * MAGN_Z_SCALE;
lpetre	0:9a72d42c0da3	56
lpetre	0:9a72d42c0da3	57	// Compensate gyroscope error
lpetre	0:9a72d42c0da3	58	gyro[0] -= GYRO_AVERAGE_OFFSET_X;
lpetre	0:9a72d42c0da3	59	gyro[1] -= GYRO_AVERAGE_OFFSET_Y;
lpetre	0:9a72d42c0da3	60	gyro[2] -= GYRO_AVERAGE_OFFSET_Z;
lpetre	0:9a72d42c0da3	61	}
lpetre	0:9a72d42c0da3	62
lpetre	0:9a72d42c0da3	63	// Reset calibration session if reset_calibration_session_flag is set
lpetre	0:9a72d42c0da3	64	void IMU::check_reset_calibration_session() {
lpetre	0:9a72d42c0da3	65	// Raw sensor values have to be read already, but no error compensation applied
lpetre	0:9a72d42c0da3	66
lpetre	0:9a72d42c0da3	67	// Reset this calibration session?
lpetre	0:9a72d42c0da3	68	if (!reset_calibration_session_flag) return;
lpetre	0:9a72d42c0da3	69
lpetre	0:9a72d42c0da3	70	// Reset acc and mag calibration variables
lpetre	0:9a72d42c0da3	71	for (int i = 0; i < 3; i++) {
lpetre	0:9a72d42c0da3	72	accel_min[i] = accel_max[i] = accel[i];
lpetre	0:9a72d42c0da3	73	magnetom_min[i] = magnetom_max[i] = magnetom[i];
lpetre	0:9a72d42c0da3	74	}
lpetre	0:9a72d42c0da3	75
lpetre	0:9a72d42c0da3	76	// Reset gyro calibration variables
lpetre	0:9a72d42c0da3	77	gyro_num_samples = 0; // Reset gyro calibration averaging
lpetre	0:9a72d42c0da3	78	gyro_average[0] = gyro_average[1] = gyro_average[2] = 0;
lpetre	0:9a72d42c0da3	79
lpetre	0:9a72d42c0da3	80	reset_calibration_session_flag = false;
lpetre	0:9a72d42c0da3	81	}
lpetre	0:9a72d42c0da3	82
lpetre	0:9a72d42c0da3	83	void IMU::turn_output_stream_on() {
lpetre	0:9a72d42c0da3	84	output_stream_on = true;
lpetre	0:9a72d42c0da3	85	statusLed = 1;
lpetre	0:9a72d42c0da3	86	}
lpetre	0:9a72d42c0da3	87
lpetre	0:9a72d42c0da3	88	void IMU::turn_output_stream_off() {
lpetre	0:9a72d42c0da3	89	output_stream_on = false;
lpetre	0:9a72d42c0da3	90	statusLed = 0;
lpetre	0:9a72d42c0da3	91	}
lpetre	0:9a72d42c0da3	92
lpetre	0:9a72d42c0da3	93	// Blocks until another byte is available on serial port
lpetre	0:9a72d42c0da3	94	char IMU::readChar() {
lpetre	0:9a72d42c0da3	95	while (pc.rxBufferGetCount() < 1) { } // Block
lpetre	0:9a72d42c0da3	96	return pc.getc();
lpetre	0:9a72d42c0da3	97	}
lpetre	0:9a72d42c0da3	98
lpetre	0:9a72d42c0da3	99	void IMU::setup() {
lpetre	0:9a72d42c0da3	100	timer.start();
lpetre	0:9a72d42c0da3	101	// Init serial output
lpetre	0:9a72d42c0da3	102	pc.baud(OUTPUT__BAUD_RATE);
lpetre	0:9a72d42c0da3	103
lpetre	0:9a72d42c0da3	104	// Init status LED
lpetre	0:9a72d42c0da3	105	statusLed = 0;
lpetre	0:9a72d42c0da3	106
lpetre	0:9a72d42c0da3	107	// Init sensors
lpetre	0:9a72d42c0da3	108	wait_ms(50); // Give sensors enough time to start
lpetre	0:9a72d42c0da3	109	I2C_Init();
lpetre	0:9a72d42c0da3	110	Accel_Init();
lpetre	0:9a72d42c0da3	111	Magn_Init();
lpetre	0:9a72d42c0da3	112	Gyro_Init();
lpetre	0:9a72d42c0da3	113
lpetre	0:9a72d42c0da3	114	// Read sensors, init DCM algorithm
lpetre	0:9a72d42c0da3	115	wait_ms(20); // Give sensors enough time to collect data
lpetre	0:9a72d42c0da3	116	reset_sensor_fusion();
lpetre	0:9a72d42c0da3	117
lpetre	0:9a72d42c0da3	118	// Init output
lpetre	0:9a72d42c0da3	119	#if (OUTPUT__HAS_RN_BLUETOOTH == true) \|\| (OUTPUT__STARTUP_STREAM_ON == false)
lpetre	0:9a72d42c0da3	120	turn_output_stream_off();
lpetre	0:9a72d42c0da3	121	#else
lpetre	0:9a72d42c0da3	122	turn_output_stream_on();
lpetre	0:9a72d42c0da3	123	#endif
lpetre	0:9a72d42c0da3	124	}
lpetre	0:9a72d42c0da3	125
lpetre	0:9a72d42c0da3	126	// Main loop
lpetre	0:9a72d42c0da3	127	void IMU::loop() {
lpetre	0:9a72d42c0da3	128
lpetre	0:9a72d42c0da3	129	// Read incoming control messages
lpetre	0:9a72d42c0da3	130	if (pc.rxBufferGetCount() >= 2) {
lpetre	0:9a72d42c0da3	131	if (pc.getc() == '#') { // Start of new control message
lpetre	0:9a72d42c0da3	132	int command = pc.getc(); // Commands
lpetre	0:9a72d42c0da3	133	if (command == 'f') // request one output _f_rame
lpetre	0:9a72d42c0da3	134	output_single_on = true;
lpetre	0:9a72d42c0da3	135	else if (command == 's') { // _s_ynch request
lpetre	0:9a72d42c0da3	136	// Read ID
lpetre	0:9a72d42c0da3	137	char id[2];
lpetre	0:9a72d42c0da3	138	id[0] = readChar();
lpetre	0:9a72d42c0da3	139	id[1] = readChar();
lpetre	0:9a72d42c0da3	140
lpetre	0:9a72d42c0da3	141	// Reply with synch message
lpetre	0:9a72d42c0da3	142	pc.printf("#SYNCH");
lpetre	0:9a72d42c0da3	143	pc.putc(id[0]);
lpetre	0:9a72d42c0da3	144	pc.putc(id[1]);
lpetre	0:9a72d42c0da3	145	pc.printf(NEW_LINE);
lpetre	0:9a72d42c0da3	146	} else if (command == 'o') { // Set _o_utput mode
lpetre	0:9a72d42c0da3	147	char output_param = readChar();
lpetre	0:9a72d42c0da3	148	if (output_param == 'n') { // Calibrate _n_ext sensor
lpetre	0:9a72d42c0da3	149	curr_calibration_sensor = (curr_calibration_sensor + 1) % 3;
lpetre	0:9a72d42c0da3	150	reset_calibration_session_flag = true;
lpetre	0:9a72d42c0da3	151	} else if (output_param == 't') // Output angles as _t_ext
lpetre	0:9a72d42c0da3	152	output_mode = OUTPUT__MODE_ANGLES_TEXT;
lpetre	0:9a72d42c0da3	153	else if (output_param == 'b') // Output angles in _b_inary form
lpetre	0:9a72d42c0da3	154	output_mode = OUTPUT__MODE_ANGLES_BINARY;
lpetre	0:9a72d42c0da3	155	else if (output_param == 'c') { // Go to _c_alibration mode
lpetre	0:9a72d42c0da3	156	output_mode = OUTPUT__MODE_CALIBRATE_SENSORS;
lpetre	0:9a72d42c0da3	157	reset_calibration_session_flag = true;
lpetre	0:9a72d42c0da3	158	} else if (output_param == 's') // Output _s_ensor values as text
lpetre	0:9a72d42c0da3	159	output_mode = OUTPUT__MODE_SENSORS_TEXT;
lpetre	0:9a72d42c0da3	160	else if (output_param == '0') { // Disable continuous streaming output
lpetre	0:9a72d42c0da3	161	turn_output_stream_off();
lpetre	0:9a72d42c0da3	162	reset_calibration_session_flag = true;
lpetre	0:9a72d42c0da3	163	} else if (output_param == '1') { // Enable continuous streaming output
lpetre	0:9a72d42c0da3	164	reset_calibration_session_flag = true;
lpetre	0:9a72d42c0da3	165	turn_output_stream_on();
lpetre	0:9a72d42c0da3	166	} else if (output_param == 'e') { // _e_rror output settings
lpetre	0:9a72d42c0da3	167	char error_param = readChar();
lpetre	0:9a72d42c0da3	168	if (error_param == '0') output_errors = false;
lpetre	0:9a72d42c0da3	169	else if (error_param == '1') output_errors = true;
lpetre	0:9a72d42c0da3	170	else if (error_param == 'c') { // get error count
lpetre	0:9a72d42c0da3	171	pc.printf("#AMG-ERR:%d,%d,%d" NEW_LINE,num_accel_errors,num_magn_errors,num_gyro_errors);
lpetre	0:9a72d42c0da3	172	}
lpetre	0:9a72d42c0da3	173	}
lpetre	0:9a72d42c0da3	174	}
lpetre	0:9a72d42c0da3	175	#if OUTPUT__HAS_RN_BLUETOOTH == true
lpetre	0:9a72d42c0da3	176	// Read messages from bluetooth module
lpetre	0:9a72d42c0da3	177	// For this to work, the connect/disconnect message prefix of the module has to be set to "#".
lpetre	0:9a72d42c0da3	178	else if (command == 'C') // Bluetooth "#CONNECT" message (does the same as "#o1")
lpetre	0:9a72d42c0da3	179	turn_output_stream_on();
lpetre	0:9a72d42c0da3	180	else if (command == 'D') // Bluetooth "#DISCONNECT" message (does the same as "#o0")
lpetre	0:9a72d42c0da3	181	turn_output_stream_off();
lpetre	0:9a72d42c0da3	182	#endif // OUTPUT__HAS_RN_BLUETOOTH == true
lpetre	0:9a72d42c0da3	183	} else { } // Skip character
lpetre	0:9a72d42c0da3	184	}
lpetre	0:9a72d42c0da3	185
lpetre	0:9a72d42c0da3	186	// Time to read the sensors again?
lpetre	0:9a72d42c0da3	187	int now = timer.read_ms();
lpetre	0:9a72d42c0da3	188	if ((now - timestamp) >= OUTPUT__DATA_INTERVAL) {
lpetre	0:9a72d42c0da3	189	timestamp_old = timestamp;
lpetre	0:9a72d42c0da3	190	timestamp = now;
lpetre	0:9a72d42c0da3	191	if (timestamp > timestamp_old)
lpetre	0:9a72d42c0da3	192	G_Dt = (float) (timestamp - timestamp_old) / 1000.0f; // Real time of loop run. We use this on the DCM algorithm (gyro integration time)
lpetre	0:9a72d42c0da3	193	else
lpetre	0:9a72d42c0da3	194	G_Dt = 0;
lpetre	0:9a72d42c0da3	195
lpetre	0:9a72d42c0da3	196	// Update sensor readings
lpetre	0:9a72d42c0da3	197	read_sensors();
lpetre	0:9a72d42c0da3	198
lpetre	0:9a72d42c0da3	199	if (output_mode == OUTPUT__MODE_CALIBRATE_SENSORS) { // We're in calibration mode
lpetre	0:9a72d42c0da3	200	check_reset_calibration_session(); // Check if this session needs a reset
lpetre	0:9a72d42c0da3	201	if (output_stream_on \|\| output_single_on)
lpetre	0:9a72d42c0da3	202	output_calibration(curr_calibration_sensor);
lpetre	0:9a72d42c0da3	203	} else if (output_mode == OUTPUT__MODE_SENSORS_TEXT) {
lpetre	0:9a72d42c0da3	204	// Apply sensor calibration
lpetre	0:9a72d42c0da3	205	compensate_sensor_errors();
lpetre	0:9a72d42c0da3	206
lpetre	0:9a72d42c0da3	207	if (output_stream_on \|\| output_single_on)
lpetre	0:9a72d42c0da3	208	output_sensors();
lpetre	0:9a72d42c0da3	209	} else if (output_mode == OUTPUT__MODE_ANGLES_TEXT \|\| output_mode == OUTPUT__MODE_ANGLES_BINARY) {
lpetre	0:9a72d42c0da3	210	// Apply sensor calibration
lpetre	0:9a72d42c0da3	211	compensate_sensor_errors();
lpetre	0:9a72d42c0da3	212
lpetre	0:9a72d42c0da3	213	// Run DCM algorithm
lpetre	0:9a72d42c0da3	214	Compass_Heading(); // Calculate magnetic heading
lpetre	0:9a72d42c0da3	215	Matrix_update();
lpetre	0:9a72d42c0da3	216	Normalize();
lpetre	0:9a72d42c0da3	217	Drift_correction();
lpetre	0:9a72d42c0da3	218	Euler_angles();
lpetre	0:9a72d42c0da3	219
lpetre	0:9a72d42c0da3	220	if (output_stream_on \|\| output_single_on)
lpetre	0:9a72d42c0da3	221	output_angles();
lpetre	0:9a72d42c0da3	222	}
lpetre	0:9a72d42c0da3	223
lpetre	0:9a72d42c0da3	224	output_single_on = false;
lpetre	0:9a72d42c0da3	225
lpetre	0:9a72d42c0da3	226	#if DEBUG__PRINT_LOOP_TIME == true
lpetre	0:9a72d42c0da3	227	Serial.print("loop time (ms) = ");
lpetre	0:9a72d42c0da3	228	Serial.println(millis() - timestamp);
lpetre	0:9a72d42c0da3	229	#endif
lpetre	0:9a72d42c0da3	230	}
lpetre	0:9a72d42c0da3	231	#if DEBUG__PRINT_LOOP_TIME == true
lpetre	0:9a72d42c0da3	232	else {
lpetre	0:9a72d42c0da3	233	Serial.println("waiting...");
lpetre	0:9a72d42c0da3	234	}
lpetre	0:9a72d42c0da3	235	#endif
lpetre	0:9a72d42c0da3	236	}

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

Razor_AHRS.cpp@0:9a72d42c0da3, 2011-12-27 (annotated)

Who changed what in which revision?

Repository toolbox

Repository details

Important Information for this Arm website

Access Warning