Hi,
I’m trying to get heading from my minimu-9. There is my code:
vector m_max = {464, 324, 408};
vector m_min = {-616, -634, -730};
void readMag(vector * mag)
{
// odczyt wartości z magnetometru
i2c_start();
i2c_write_byte(0x3C); // write mag
i2c_write_byte(0x03); // OUTXH_M, MSB set to enable auto-increment
i2c_start(); // repeated start
i2c_write_byte(0x3D); // read mag
unsigned char mxh = i2c_read_byte();
unsigned char mxl = i2c_read_byte();
unsigned char myh = i2c_read_byte();
unsigned char myl = i2c_read_byte();
unsigned char mzh = i2c_read_byte();
unsigned char mzl = i2c_read_last_byte();
i2c_stop();
mag->x = (int16_t)(mxh << 8 | mxl);
mag->y = (int16_t)(myh << 8 | myl);
mag->z = (int16_t)(mzh << 8 | mzl);
}
void readAcc(vector *acc)
{
// odczyt wartości z akcelerometru
i2c_start();
i2c_write_byte(0x30); // write acc
i2c_write_byte(0xa8); // OUT_X_L_A, MSB set to enable auto-increment
i2c_start(); // repeated start
i2c_write_byte(0x31); // 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();
acc->x = ((int16_t)(axh << 8 | axl)) >> 4;
acc->y = ((int16_t)(ayh << 8 | ayh)) >> 4;
acc->z = ((int16_t)(azh << 8 | azl)) >> 4;
}
void vector_cross(const vector *a, const vector *b, vector *out)
{
out->x = a->y * b->z - a->z * b->y;
out->y = a->z * b->x - a->x * b->z;
out->z = a->x * b->y - a->y * b->x;
}
float vector_dot(const vector *a, const vector *b)
{
return a->x * b->x + a->y * b->y + a->z * b->z;
}
void vector_normalize(vector *a)
{
float mag = sqrt(vector_dot(a, a));
a->x /= mag;
a->y /= mag;
a->z /= mag;
}
int heading(vector from)
{
vector a;
vector m;
readAcc(&a);
readMag(&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;
vector temp_a = a;
// normalize
vector_normalize(&temp_a);
//vector_normalize(&m);
// compute E and N
vector E;
vector N;
vector_cross(&m, &temp_a, &E);
vector_normalize(&E);
vector_cross(&temp_a, &E, &N);
// compute heading
int heading = round(atan2(vector_dot(&E, &from), vector_dot(&N, &from)) * 180 / M_PI);
if (heading < 0) heading += 360;
return heading;
}I’m sure my values vector m_max = {464, 324, 408} vector m_min = {-616, -634, -730}; are correct(I made a calibration).
What’s problem with my program? When did I turn around my minimu, the angles wasn’t between 0…359, but there were values 0…40…0 and 330…360…330.
What’s wrong with my code?
Thanks for reply
Best regards