I’ve been at this for a couple days and I believe my code is solid. I’m using the AltIMU10 with a raspberry pi v2 (ic2 Bus 1). The AltMU10 v4 uses the LSM303D sensor array. I’ve been working on the math quite a bit and I cannot seem to get anything resembling good directional data from the compass sensors. I have no reason to believe the sensor data is bad, I ran calibration and have those values stored. To provide comparisons I placed a cell phone, with the GPS deactivated, in the same position as my sensor to get a reference reading on around where everything should be calculating. My data is all over the place, I get readings that span from 303 deg to 14 deg. A couple of the columns appear to be cumulative where they shouldn’t be. Can anyone offer some insight as to where my readings are going astray? If it is something simple, berate me but please give me an idea of where to look, a hint would be just as helpful as an answer. I can provide the header files I created but they are all definitions defined by the LSM303D datasheet.
#include <iostream>
#include <stdio.h>
#include <stdint.h>
#include <string.h>
#include <bitset>
#include <linux/i2c-dev.h>
#include <fcntl.h>
#include <math.h>
#include "/home/pi/Desktop/currentProject/altIMU10.h"
using namespace std;
int file;
int smoothed_output[6];
float lowpass_val = 0.015f;
void shutdown_sensors();
void initialize_sensors();
void ReadTemp();
void checkHeading();
void RegWrite(uint8_t reg, uint8_t regValue);
void readMag(int *m);
void ReadReg(uint8_t command, uint8_t size, uint8_t *data);
void ReadTilt(uint8_t command, uint8_t size, uint8_t *data);
int selDevice(uint8_t device);
int main()
{
cout << "Program started. \n" << endl;
char filename[20] = "/dev/i2c-1";
int n = 0;
file = open (filename, O_RDWR);
if (file<0)
{
printf("unable to open I2C Bus ");
perror("The following error occured");
return(1);
}
else
{
cout << "Bus Opened" << endl;
}
initialize_sensors();
// AI and sensor reading loop.
while (n < 30)
{
selDevice(MAG_ADD);
ReadTemp();
checkHeading();
sleep(1);
//usleep(50000);
n++;
}
// End of AI and sensor loop.
shutdown_sensors();
return 0;
}
void initialize_sensors()
{
printf("Initializing Sensors...\n");
if (ioctl(file, I2C_SLAVE, MAG_ADD) <0)
{
printf("Magnometer failed!\n");
}
else
{
RegWrite(MAG_CTRL1, 0b01000111);
RegWrite(MAG_CTRL5, 0b11110000);
RegWrite(MAG_CTRL7, 0b10000000);
RegWrite(MAG_CTRL6, 0b00100000);
RegWrite(MAG_OFFSET_X_L_M,0b01100111);
RegWrite(MAG_OFFSET_Y_L_M, 0b10111001);
RegWrite(MAG_OFFSET_Y_H_M, 0b11111111);
RegWrite(MAG_OFFSET_Z_L_M, 0b01011101);
printf("Magnometer initialized.\n");
}
}
void RegWrite(uint8_t reg, uint8_t regValue)
{
int result =0;
result = i2c_smbus_write_byte_data(file,reg,regValue);
if (result == -1)
{
printf("Failure writing to register.\n");
}
}
void ReadReg(uint8_t command, uint8_t size, uint8_t *data)
{
int result = i2c_smbus_read_i2c_block_data(file,command, size, data);
if (result != size)
{
printf("Reading Failed. \n");
}
}
void shutdown_sensors()
{
printf("Shutting down sensors...\n");
if (ioctl(file, I2C_SLAVE, MAG_ADD) <0)
{
printf("Magnometer not shut down!\n");
}
else
{
RegWrite(MAG_CTRL1, 0b00000111);
RegWrite(MAG_CTRL5, 0b00011000);
RegWrite(MAG_CTRL7, 0b00000010);
printf("Magnometer shut down.\n");
}
}
int selDevice(uint8_t device)
{
int result = ioctl(file, I2C_SLAVE, device);
return result;
}
void ReadTemp()
{
uint8_t block[2];
uint16_t temp;
int display_temp;
ReadReg(0x80 | MAG_TEMP_OUT_L, sizeof(block), block);
temp = (((block[0] << 8) | block[1]) >> 4) * .125;
display_temp = ((temp*9)/5) + 32;
printf("%d F \t",display_temp);
}
void checkHeading()
{
int magRaw[6];
float heading;
readMag(magRaw);
int mX = magRaw[0];
int mY = magRaw[1];
int mZ = magRaw[2];
int aX = magRaw[3];
int aY = magRaw[4];
int aZ = magRaw[5];
float xh;
float yh;
float ayf;
float axf;
ayf = aY/57.0; //convert to radians
axf = aX/57.0; //convert to radians
xh = mX*cos(ayf) + mY*sin(axf)-mZ*cos(axf)*sin(ayf);
yh = mY*cos(axf) + mZ*sin(axf);
heading = atan2((double)yh,(double)xh) * (180/PI ) -90; //angle in degrees
if (heading > 0){heading = heading - 360;}
heading = 360 + heading;
printf("%7.3f deg\t x %i \t y %i \t z %i \n", heading, mX,mY,mZ);
//cout.flush();
}
void readMag(int *m)
{
uint8_t block[6];
uint8_t accBlock[6];
int newData[6];
int x;
ReadReg(0x80 | MAG_OUT_X_L_M, sizeof(block), block);
ReadReg(0x80 | MAG_OUT_X_L_A, sizeof(accBlock),accBlock);
newData[0] = ((int)(block[0] << 8 | block[1]));
newData[1] = ((int)(block[2] << 8 | block[3]));
newData[2] = ((int)(block[4] << 8 | block[5]));
newData[3] = ((int)(accBlock[0] << 8 | accBlock[1]));
newData[4] = ((int)(accBlock[2] << 8 | accBlock[3]));
newData[5] = ((int)(accBlock[4] << 8 | accBlock[5]));
if (smoothed_output[0] == 0 && smoothed_output[1] == 0 && smoothed_output[2] ==0 && smoothed_output[3] == 0 && smoothed_output[4] == 0 && smoothed_output[5] == 0)
{
for (int i = 0; i < sizeof(newData); i++)
{
smoothed_output[i] = newData[i];
}
}
else
{
for (int i = 0; i < sizeof(newData); i++)
{
smoothed_output[i] = smoothed_output[i] + lowpass_val * (newData[i] - smoothed_output[i]);
}
}
*m = smoothed_output[0];
*(m+1) = smoothed_output[1];
*(m+2) = smoothed_output[2];
*(m+3) = smoothed_output[3];
*(m+4) = smoothed_output[4];
*(m+5) = smoothed_output[5];
cout << newData[3] << endl;
}
Thank you for any help.
