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

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

Committer:: lpetre
Date:: Tue Dec 27 17:20:06 2011 +0000
Revision:: 0:9a72d42c0da3
Child:: 1:e27c4c0b71d8

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
lpetre	0:9a72d42c0da3	4	// I2C code to read the sensors
lpetre	0:9a72d42c0da3	5
lpetre	0:9a72d42c0da3	6	// Sensor I2C addresses
lpetre	0:9a72d42c0da3	7	#define ACCEL_READ 0xA7
lpetre	0:9a72d42c0da3	8	#define ACCEL_WRITE 0xA6
lpetre	0:9a72d42c0da3	9	#define MAGN_WRITE 0x3C
lpetre	0:9a72d42c0da3	10	#define MAGN_READ 0x3D
lpetre	0:9a72d42c0da3	11	#define GYRO_WRITE 0xD0
lpetre	0:9a72d42c0da3	12	#define GYRO_READ 0xD1
lpetre	0:9a72d42c0da3	13
lpetre	0:9a72d42c0da3	14	void IMU::I2C_Init()
lpetre	0:9a72d42c0da3	15	{
lpetre	0:9a72d42c0da3	16	Wire.frequency(100000);
lpetre	0:9a72d42c0da3	17	}
lpetre	0:9a72d42c0da3	18
lpetre	0:9a72d42c0da3	19	void IMU::Accel_Init()
lpetre	0:9a72d42c0da3	20	{
lpetre	0:9a72d42c0da3	21	char tx[2];
lpetre	0:9a72d42c0da3	22	tx[0] = 0x2D; // Power register
lpetre	0:9a72d42c0da3	23	tx[1] = 0x08; // Power register
lpetre	0:9a72d42c0da3	24	Wire.write(ACCEL_WRITE, tx, 2);
lpetre	0:9a72d42c0da3	25	wait_ms(5);
lpetre	0:9a72d42c0da3	26	tx[0] = 0x31; // Data format register
lpetre	0:9a72d42c0da3	27	tx[1] = 0x08; // Set to full resolution
lpetre	0:9a72d42c0da3	28	Wire.write(ACCEL_WRITE, tx, 2);
lpetre	0:9a72d42c0da3	29	wait_ms(5);
lpetre	0:9a72d42c0da3	30
lpetre	0:9a72d42c0da3	31	// Because our main loop runs at 50Hz we adjust the output data rate to 50Hz (25Hz bandwidth)
lpetre	0:9a72d42c0da3	32	tx[0] = 0x2C; // Rate
lpetre	0:9a72d42c0da3	33	tx[1] = 0x09; // Set to 50Hz, normal operation
lpetre	0:9a72d42c0da3	34	Wire.write(ACCEL_WRITE, tx, 2);
lpetre	0:9a72d42c0da3	35	wait_ms(5);
lpetre	0:9a72d42c0da3	36	}
lpetre	0:9a72d42c0da3	37
lpetre	0:9a72d42c0da3	38	// Reads x, y and z accelerometer registers
lpetre	0:9a72d42c0da3	39	void IMU::Read_Accel()
lpetre	0:9a72d42c0da3	40	{
lpetre	0:9a72d42c0da3	41	char buff[6];
lpetre	0:9a72d42c0da3	42	char tx = 0x32; // Send address to read from
lpetre	0:9a72d42c0da3	43
lpetre	0:9a72d42c0da3	44	Wire.write(ACCEL_WRITE, &tx, 1);
lpetre	0:9a72d42c0da3	45
lpetre	0:9a72d42c0da3	46	if (Wire.read(ACCEL_READ, buff, 6) == 0) // All bytes received?
lpetre	0:9a72d42c0da3	47	{
lpetre	0:9a72d42c0da3	48	// No multiply by -1 for coordinate system transformation here, because of double negation:
lpetre	0:9a72d42c0da3	49	// We want the gravity vector, which is negated acceleration vector.
lpetre	0:9a72d42c0da3	50	accel[0] = (int)buff[3] << 8 \| (int)buff[2]; // X axis (internal sensor y axis)
lpetre	0:9a72d42c0da3	51	accel[1] = (int)buff[1] << 8 \| (int)buff[0]; // Y axis (internal sensor x axis)
lpetre	0:9a72d42c0da3	52	accel[2] = (int)buff[5] << 8 \| (int)buff[4]; // Z axis (internal sensor z axis)
lpetre	0:9a72d42c0da3	53	}
lpetre	0:9a72d42c0da3	54	else
lpetre	0:9a72d42c0da3	55	{
lpetre	0:9a72d42c0da3	56	num_accel_errors++;
lpetre	0:9a72d42c0da3	57	if (output_errors) pc.printf("!ERR: reading accelerometer" NEW_LINE);
lpetre	0:9a72d42c0da3	58	}
lpetre	0:9a72d42c0da3	59	}
lpetre	0:9a72d42c0da3	60
lpetre	0:9a72d42c0da3	61	void IMU::Magn_Init()
lpetre	0:9a72d42c0da3	62	{
lpetre	0:9a72d42c0da3	63	char tx[2];
lpetre	0:9a72d42c0da3	64	tx[0] = 0x02; // Mode
lpetre	0:9a72d42c0da3	65	tx[1] = 0x00; // Set continuous mode (default 10Hz)
lpetre	0:9a72d42c0da3	66	Wire.write(MAGN_WRITE, tx, 2);
lpetre	0:9a72d42c0da3	67	wait_ms(5);
lpetre	0:9a72d42c0da3	68
lpetre	0:9a72d42c0da3	69	tx[0] = 0x00; // CONFIG_A
lpetre	0:9a72d42c0da3	70	tx[1] = 0x18; // Set 50Hz
lpetre	0:9a72d42c0da3	71	Wire.write(MAGN_WRITE, tx, 2);
lpetre	0:9a72d42c0da3	72	wait_ms(5);
lpetre	0:9a72d42c0da3	73	}
lpetre	0:9a72d42c0da3	74
lpetre	0:9a72d42c0da3	75	void IMU::Read_Magn()
lpetre	0:9a72d42c0da3	76	{
lpetre	0:9a72d42c0da3	77	char buff[6];
lpetre	0:9a72d42c0da3	78	char tx = 0x03; // Send address to read from
lpetre	0:9a72d42c0da3	79
lpetre	0:9a72d42c0da3	80	Wire.write(MAGN_WRITE, &tx, 1);
lpetre	0:9a72d42c0da3	81
lpetre	0:9a72d42c0da3	82	if (Wire.read(MAGN_READ, buff, 6) == 0) // All bytes received?
lpetre	0:9a72d42c0da3	83	{
lpetre	0:9a72d42c0da3	84	// 9DOF Razor IMU SEN-10125 using HMC5843 magnetometer
lpetre	0:9a72d42c0da3	85	#if HW__VERSION_CODE == 10125
lpetre	0:9a72d42c0da3	86	// MSB byte first, then LSB; X, Y, Z
lpetre	0:9a72d42c0da3	87	magnetom[0] = -1 * (((int) buff[2]) << 8) \| buff[3]; // X axis (internal sensor -y axis)
lpetre	0:9a72d42c0da3	88	magnetom[1] = -1 * (((int) buff[0]) << 8) \| buff[1]; // Y axis (internal sensor -x axis)
lpetre	0:9a72d42c0da3	89	magnetom[2] = -1 * (((int) buff[4]) << 8) \| buff[5]; // Z axis (internal sensor -z axis)
lpetre	0:9a72d42c0da3	90	// 9DOF Razor IMU SEN-10736 using HMC5883L magnetometer
lpetre	0:9a72d42c0da3	91	#elif HW__VERSION_CODE == 10736
lpetre	0:9a72d42c0da3	92	// MSB byte first, then LSB; Y and Z reversed: X, Z, Y
lpetre	0:9a72d42c0da3	93	magnetom[0] = -1 * (((int) buff[4]) << 8) \| buff[5]; // X axis (internal sensor -y axis)
lpetre	0:9a72d42c0da3	94	magnetom[1] = -1 * (((int) buff[0]) << 8) \| buff[1]; // Y axis (internal sensor -x axis)
lpetre	0:9a72d42c0da3	95	magnetom[2] = -1 * (((int) buff[2]) << 8) \| buff[3]; // Z axis (internal sensor -z axis)
lpetre	0:9a72d42c0da3	96	// 9DOF Sensor Stick SEN-10183 and SEN-10321 using HMC5843 magnetometer
lpetre	0:9a72d42c0da3	97	#elif (HW__VERSION_CODE == 10183) \|\| (HW__VERSION_CODE == 10321)
lpetre	0:9a72d42c0da3	98	// MSB byte first, then LSB; X, Y, Z
lpetre	0:9a72d42c0da3	99	magnetom[0] = (((int) buff[0]) << 8) \| buff[1]; // X axis (internal sensor x axis)
lpetre	0:9a72d42c0da3	100	magnetom[1] = -1 * (((int) buff[2]) << 8) \| buff[3]; // Y axis (internal sensor -y axis)
lpetre	0:9a72d42c0da3	101	magnetom[2] = -1 * (((int) buff[4]) << 8) \| buff[5]; // Z axis (internal sensor -z axis)
lpetre	0:9a72d42c0da3	102	// 9DOF Sensor Stick SEN-10724 using HMC5883L magnetometer
lpetre	0:9a72d42c0da3	103	#elif HW__VERSION_CODE == 10724
lpetre	0:9a72d42c0da3	104	// MSB byte first, then LSB; Y and Z reversed: X, Z, Y
lpetre	0:9a72d42c0da3	105	magnetom[0] = 1 * ((int)buff[0] << 8 \| (int)buff[1]); // X axis (internal sensor x axis)
lpetre	0:9a72d42c0da3	106	magnetom[1] = -1 * ((int)buff[4] << 8 \| (int)buff[5]); // Y axis (internal sensor -y axis)
lpetre	0:9a72d42c0da3	107	magnetom[2] = -1 * ((int)buff[2] << 8 \| (int)buff[3]); // Z axis (internal sensor -z axis)
lpetre	0:9a72d42c0da3	108	#endif
lpetre	0:9a72d42c0da3	109	}
lpetre	0:9a72d42c0da3	110	else
lpetre	0:9a72d42c0da3	111	{
lpetre	0:9a72d42c0da3	112	num_magn_errors++;
lpetre	0:9a72d42c0da3	113	if (output_errors) pc.printf("!ERR: reading magnetometer" NEW_LINE);
lpetre	0:9a72d42c0da3	114	}
lpetre	0:9a72d42c0da3	115	}
lpetre	0:9a72d42c0da3	116
lpetre	0:9a72d42c0da3	117	void IMU::Gyro_Init()
lpetre	0:9a72d42c0da3	118	{
lpetre	0:9a72d42c0da3	119	char tx[2];
lpetre	0:9a72d42c0da3	120
lpetre	0:9a72d42c0da3	121	// Power up reset defaults
lpetre	0:9a72d42c0da3	122	tx[0] = 0x3E; // Power management
lpetre	0:9a72d42c0da3	123	tx[1] = 0x80; // ?
lpetre	0:9a72d42c0da3	124	Wire.write(GYRO_WRITE, tx, 2);
lpetre	0:9a72d42c0da3	125	wait_ms(5);
lpetre	0:9a72d42c0da3	126
lpetre	0:9a72d42c0da3	127	// Select full-scale range of the gyro sensors
lpetre	0:9a72d42c0da3	128	// Set LP filter bandwidth to 42Hz
lpetre	0:9a72d42c0da3	129	tx[0] = 0x16; //
lpetre	0:9a72d42c0da3	130	tx[1] = 0x1B; // DLPF_CFG = 3, FS_SEL = 3
lpetre	0:9a72d42c0da3	131	Wire.write(GYRO_WRITE, tx, 2);
lpetre	0:9a72d42c0da3	132	wait_ms(5);
lpetre	0:9a72d42c0da3	133
lpetre	0:9a72d42c0da3	134	// Set sample rato to 50Hz
lpetre	0:9a72d42c0da3	135	tx[0] = 0x15; //
lpetre	0:9a72d42c0da3	136	tx[1] = 0x0A; // SMPLRT_DIV = 10 (50Hz)
lpetre	0:9a72d42c0da3	137	Wire.write(GYRO_WRITE, tx, 2);
lpetre	0:9a72d42c0da3	138	wait_ms(5);
lpetre	0:9a72d42c0da3	139
lpetre	0:9a72d42c0da3	140	// Set clock to PLL with z gyro reference
lpetre	0:9a72d42c0da3	141	tx[0] = 0x3E;
lpetre	0:9a72d42c0da3	142	tx[1] = 0x00;
lpetre	0:9a72d42c0da3	143	Wire.write(GYRO_WRITE, tx, 2);
lpetre	0:9a72d42c0da3	144	wait_ms(5);
lpetre	0:9a72d42c0da3	145	}
lpetre	0:9a72d42c0da3	146
lpetre	0:9a72d42c0da3	147	// Reads x, y and z gyroscope registers
lpetre	0:9a72d42c0da3	148	void IMU::Read_Gyro()
lpetre	0:9a72d42c0da3	149	{
lpetre	0:9a72d42c0da3	150	char buff[6];
lpetre	0:9a72d42c0da3	151	char tx = 0x1D; // Sends address to read from
lpetre	0:9a72d42c0da3	152
lpetre	0:9a72d42c0da3	153	Wire.write(GYRO_WRITE, &tx, 1);
lpetre	0:9a72d42c0da3	154
lpetre	0:9a72d42c0da3	155	if (Wire.read(GYRO_READ, buff, 6) == 0) // All bytes received?
lpetre	0:9a72d42c0da3	156	{
lpetre	0:9a72d42c0da3	157	gyro[0] = -1 * ((int)buff[2] << 8 \| (int)buff[3]); // X axis (internal sensor -y axis)
lpetre	0:9a72d42c0da3	158	gyro[1] = -1 * ((int)buff[0] << 8 \| (int)buff[1]); // Y axis (internal sensor -x axis)
lpetre	0:9a72d42c0da3	159	gyro[2] = -1 * ((int)buff[4] << 8 \| (int)buff[5]); // Z axis (internal sensor -z axis)
lpetre	0:9a72d42c0da3	160	}
lpetre	0:9a72d42c0da3	161	else
lpetre	0:9a72d42c0da3	162	{
lpetre	0:9a72d42c0da3	163	num_gyro_errors++;
lpetre	0:9a72d42c0da3	164	if (output_errors) pc.printf("!ERR: reading gyroscope" NEW_LINE);
lpetre	0:9a72d42c0da3	165	}
lpetre	0:9a72d42c0da3	166	}

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

Sensors.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