90 degree offset with LSM303DLHC (using C)

Robotics and electronics discussion Software and microcontrollers
1

Hello all,

This is my first post… usually I just search the forums, but I haven’t had any luck with searching for someone with this issue. In summary, I recently ported a project I’m working on from an Arduino with a LSM303DLM daughter board to a custom PCB running an ATMEGA644PA and an LSM303DLHC. The problem I am having is the final heading result is now off by 90 degrees (when the x-axis is pointing north, the heading is telling me I’m facing east).

Has anybody seen anything similar to this issue?

A couple of notes:

  1. I am aware of the new accelerometer address, and can successfully communicate with it via I2C.
  2. I have the high resolution bit enabled (but I have also tried it without this enabled).
  3. I downloaded the LSM303DLHC arduino library, and took note of the differences in the code vs. the LSM303DLM library. I believe I’ve updated my code accordingly.
  4. To the best of my knowledge, I’ve calibrated the compass correctly (the same as I did with the LSM303DLM, which worked fine).

Here are the relevant snippets of code:

static void smrt_io_compass_init()
{
	TWSR = 0;  // clear bit-rate prescale bits
	TWBR = 17; // produces an SCL frequency of 400 kHz with a 20 MHz CPU clock speed

	//enable accelerometer
	i2c_start();
	i2c_write_byte(ACC_ADDRESS); // write acc
	i2c_write_byte(0x20); // CTRL_REG1_A
	i2c_write_byte(0x77); // normal power mode, 400 Hz data rate, all axes enabled

	i2c_start();
	i2c_write_byte(ACC_ADDRESS);
	i2c_write_byte(0x23); // CTRL_REG4_A
	i2c_write_byte(0x8);  // enable high resolution mode
	i2c_stop();

	//enable magnetometer
	i2c_start();
	i2c_write_byte(MAG_ADDRESS); // write mag
	i2c_write_byte(0x02); // MR_REG_M
	i2c_write_byte(0x00); // continuous conversion mode
	i2c_stop();
}

void smrt_io_read_data(vector *a, vector *m)
{
	smrt_io_read_data_raw(a, m);

	// shift and scale
	m->x = (m->x - m_min.x) / (m_max.x - m_min.x) * 2 - 1.0;
	m->y = (m->y - m_min.y) / (m_max.y - m_min.y) * 2 - 1.0;
	m->z = (m->z - m_min.z) / (m_max.z - m_min.z) * 2 - 1.0;
}

static void smrt_io_read_data_raw(vector *a, vector *m)
{
	// read accelerometer values
	i2c_start();
	i2c_write_byte(ACC_ADDRESS); // write acc
	i2c_write_byte(0xa8); // OUT_X_L_A, MSB set to enable auto-increment
	i2c_start();		  // repeated start
	i2c_write_byte(ACC_ADDRESS+1); // read acc
	unsigned char axl = i2c_read_byte();
	unsigned char axh = i2c_read_byte();
	unsigned char ayl = i2c_read_byte();
	unsigned char ayh = i2c_read_byte();
	unsigned char azl = i2c_read_byte();
	unsigned char azh = i2c_read_last_byte();
	i2c_stop();

	// read magnetometer values
	i2c_start();
	i2c_write_byte(MAG_ADDRESS); // write mag
	i2c_write_byte(0x03); // OUTXH_M
	i2c_start();		  // repeated start
	i2c_write_byte(MAG_ADDRESS + 1); // read mag
	unsigned char mxh = i2c_read_byte();
	unsigned char mxl = i2c_read_byte();
	// for the DLM and DLHC, the register address for Z comes before Y
	unsigned char mzh = i2c_read_byte();
	unsigned char mzl = i2c_read_byte();
	unsigned char myh = i2c_read_byte();
	unsigned char myl = i2c_read_last_byte();
	i2c_stop();

  // combine high and low bytes, then shift right to discard lowest 4 bits (which are meaningless)
  // GCC performs an arithmetic right shift for signed negative numbers, but this code will not work
  // if you port it to a compiler that does a logical right shift instead.
  a->x = ((int16_t)(axh << 8 | axl)) >> 4;
  a->y = ((int16_t)(ayh << 8 | ayl)) >> 4;
  a->z = ((int16_t)(azh << 8 | azl)) >> 4;

	m->x = mxh << 8 | mxl;
	m->y = myh << 8 | myl;
	m->z = mzh << 8 | mzl;
}

// Returns a heading (in degrees) given an acceleration vector a due to gravity, a magnetic vector m, and a facing vector p.
int smrt_io_get_heading(const vector *a, const vector *m, const vector *p)
{
	vector E;
	vector N;

	vector_normalize(a);

	// cross magnetic vector (magnetic north + inclination) with "down" (acceleration vector) to produce "east"
	vector_cross(m, a, &E);
	vector_normalize(&E);

	// cross "down" with "east" to produce "north" (parallel to the ground)
	vector_cross(a, &E, &N);

	// compute heading
	int heading = round(atan2(vector_dot(&E, p), vector_dot(&N, p)) * 180 / M_PI);
	if (heading < 0)
	heading += 360;
	return heading;
}
2

What procedure did you use to “calibrate the compass correctly”? Did the problem you report appear at that time? If not, when did the problem appear?

BTW I recommend the second procedure described in the following post(s) for calibrating the magnetometer: sailboatinstruments.blogspot.com … ation.html

Finally, whoever wrote the original code is clearly confused about the right-hand convention for the vector cross product. It is a shame that the comments in the code have been allowed to stand for so long (i.e. cross “north” with “down” to produce “east” – N X D => W).

3

Unfortunately, this problem appeared from the start (with the factory calibration). However, to calibrate, I am using a ported version of the Arduino calibration algorithm.

The tool in the link you provided looks useful, and not to painful to use. I think I will give that a try to be sure that my issue isn’t with the calibration.

Thanks.

4

Are you certain that you are orienting the PCB correctly? For some reason, north is defined in the code as the -Y direction. In any case, if everything else is working correctly, it is trivial to redefine north with respect to the PCB orientation.

5

Jim,

I think you are correct… when the PCB was laid out, it was placed where the heading calculation desired is the X direction. However, the vector ‘p’ initialization remained at {0,-1,0} (I don’t remember now, but my old Arduino setup must have had -Y as the heading reference). I believe that’s the issue. I figured it was down in that heading calculation area, but I never even thought to check the ‘p’ vector initialiazation.

Thank you very much for your help.

6

Hello, cazzy.

I’m glad you and Jim worked it out. The heading() function returns the heading of the -Y axis (the angular difference between the -Y axis and north in the horizontal plane) by default because that’s the axis that points away from you if you orient our carrier boards the way our pictures show:

If you want to use a different axis as a “pointer”, you can use the version of the heading() function that takes a user-defined vector. In your case, you can just use:

heading((vector){1,0,0});

Jim, you are right that I should have corrected the comments for the heading() function in the library long ago. The version on GitHub should be fixed now.

- Kevin

7

Thank you for confirming… this forum has been helpful (I’m sure I’ll be back!). Yep, the change to {1,0,0} has already been made. Thanks again.