Hello guys. I am making compass for robotic vehicle using LSM303D. It suppose to drive over field, so I have to tilt compensate the device. I am making program in C++ for Raspberry Pi 3:
#include <iostream>
#include <pigpio.h>
#include <unistd.h>
#include <cmath>
#include <algorithm>
#include <string>
#include <fstream>
#include <vector>
/*
Connect Pi 3V3 - VCC, Ground - Ground, SDA - SDA, SCL - SCL.
*/
#define LSM303D_I2C_ADDR 0x1d
#define LSM303D_WHOAMI 73
#define busNum 1
//--- Control register addresses -- from LSM303D datasheet
#define CTRL_0 31 //--- General settings
#define CTRL_1 32 //--- Turns on accelerometer and configures data rate
#define CTRL_2 33 //--- Self test accelerometer, anti-aliasing accel filter
#define CTRL_3 34 //--- Interrupts
#define CTRL_4 35 //--- Interrupts
#define CTRL_5 36 //--- Turns on temperature sensor
#define CTRL_6 37 //--- Magnetic resolution selection, data rate config
#define CTRL_7 38 //--- Turns on magnetometer and adjusts mode
//--- Registers holding twos-complemented MSB and LSB of magnetometer readings -- from LSM303D datasheet
#define MAG_X_LSB 8 //--- x
#define MAG_X_MSB 9
#define MAG_Y_LSB 10 //--- y
#define MAG_Y_MSB 11
#define MAG_Z_LSB 12 //--- z
#define MAG_Z_MSB 13
//--- Registers holding twos-complemented MSB and LSB of magnetometer readings -- from LSM303D datasheet
#define ACC_X_LSB 40 //--- x
#define ACC_X_MSB 41
#define ACC_Y_LSB 42 //--- y
#define ACC_Y_MSB 43
#define ACC_Z_LSB 44 //--- z
#define ACC_Z_MSB 45
//--- Registers holding 12-bit right justified, twos-complemented temperature data -- from LSM303D datasheet
#define TEMP_MSB 5
#define TEMP_LSB 6
//--- Deklinacja magnetyczna
#define MAG_DECLINATION 82.32
//--- Do kalibracji
#define magXmax 728.627
#define magYmax 1030.94
#define magXmin -1282
#define magYmin -948.876
int combineTwoRegister(int MSB, int LSB);
std::vector <double> findCalibration(int handle);
int main()
{
//--- Variables
int magX, magY, magZ;
int accX, accY, accZ;
int handle;
double heading, declination;
double accXnorm, accYnorm, pitch, roll, magXcomp, magYcomp, magZcomp, magXheading, magYheading;
std::ofstream file;
std::vector<double> calibration;
file.open("compass.txt");
if(file.good() == true)
{
std::cout << "Access granted!" << std::endl;
} else {
i2cClose(handle);
gpioTerminate();
std::cout << "Access denied!" << std::endl;
exit(-1);
}
//--- Initialisation
if (gpioInitialise() < 0){
std::cout<<"Cannot initialise PIGPIO"<<std::endl;
return 1;
}
handle = i2cOpen(1, LSM303D_I2C_ADDR, 0);
if(handle >= 0){
std::cout << "I2C set" << std::endl;
} else {
std::cout << "Cannot set I2C" << std::endl;
}
if (i2cReadByteData(handle, 15) == LSM303D_WHOAMI){
std::cout << "LSM303D detected successfully" << std::endl;
} else {
std::cout << "No LSM303D detected on bus " << busNum << " . Found: " << i2cReadByteData(handle, 15) << std::endl;
}
//--- Registers
i2cWriteByteData(handle, CTRL_1, 0x57);
i2cWriteByteData(handle, CTRL_2, 0x00);
i2cWriteByteData(handle, CTRL_5, 0x64);
i2cWriteByteData(handle, CTRL_6, 0x20);
i2cWriteByteData(handle, CTRL_7, 0x00);
usleep(5000000);
calibration = findCalibration(handle);
while(true){
std::cout<<"Odczyt danych z czujnika:" <<std::endl;
//--- Magnetic Data
magX = combineTwoRegister(i2cReadByteData(handle, MAG_X_MSB), i2cReadByteData(handle, MAG_X_LSB));
magY = combineTwoRegister(i2cReadByteData(handle, MAG_Y_MSB), i2cReadByteData(handle, MAG_Y_LSB));
magZ = combineTwoRegister(i2cReadByteData(handle, MAG_Z_MSB), i2cReadByteData(handle, MAG_Z_MSB));
//magY = -magY;
//--- Acceleration Data
accX = combineTwoRegister(i2cReadByteData(handle, ACC_X_MSB), i2cReadByteData(handle, ACC_X_LSB));
accY = combineTwoRegister(i2cReadByteData(handle, ACC_Y_MSB), i2cReadByteData(handle, ACC_Y_LSB));
accZ = combineTwoRegister(i2cReadByteData(handle, ACC_Z_MSB), i2cReadByteData(handle, ACC_Z_MSB));
//accX = -accX;
//accY = -accY;
//--- Pitch and Roll calculation
accXnorm = accX/sqrt(accX * accX + accY * accY + accZ * accZ);
accYnorm = accY/sqrt(accX * accX + accY * accY + accZ * accZ);
pitch = asin(-accXnorm);
roll = asin(accYnorm/cos(pitch));
//--- Hard Iron Compensation
//magXcomp -= (magXmin + magXmax) / 2; - OLD VERSION
//magYcomp -= (magYmin + magYmax) / 2;
magXcomp = (magX-calibration[0]) / (calibration[1]-calibration[0]) * 2 - 1;
magYcomp = (magY-calibration[2]) / (calibration[3]-calibration[2]) * 2 - 1;
magZcomp = (magZ-calibration[4]) / (calibration[5]-calibration[4]) * 2 - 1;
//--- Final data
magXheading = magXcomp * cos(pitch) + magZcomp * sin(pitch);
magYheading = magXcomp * sin(roll) * sin(pitch) + magYcomp * cos(roll) - magZcomp * sin(roll) * cos(pitch);
//--- Declination calculation
declination = MAG_DECLINATION / 1000.0;
/*std::cout << "MagX: " << magXcomp << " MagY: " << magYcomp << " MagZ: " << magZ << std::endl;
file << magXcomp << ", " << magYcomp << std::endl;*/
//--- Heading calculation
heading = 180 * atan2(magYheading,magXheading)/M_PI;
heading += declination * 180/M_PI;
if(heading < 0){
heading += 360;
}
std::cout << "Heading: " << heading << std::endl;
usleep(250000);
std::system("clear");
}
//--- Closing all
i2cClose(handle);
gpioTerminate();
return (0);
}
int combineTwoRegister(int MSB, int LSB){
//std::cout<<"MSB: " << MSB << " LSB: " << LSB<<std::endl;
int combined = 256 * MSB + LSB;
if(combined >= 32768){
return combined - 65536;
} else {
return combined;
}
}
std::vector <double> findCalibration(int handle){
std::vector<double> temp;
std::vector<double> results;
double mag;
int samples = 5000;
for (int i = 0; i < samples; i++){
mag = combineTwoRegister(i2cReadByteData(handle, MAG_X_MSB), i2cReadByteData(handle, MAG_X_LSB));
temp.push_back(mag);
}
double max = *max_element(temp.begin(), temp.end());
double min = *min_element(temp.begin(), temp.end());
temp.clear();
results.push_back(min);
results.push_back(max);
for (int i = 0; i < samples; i++){
mag = combineTwoRegister(i2cReadByteData(handle, MAG_Y_MSB), i2cReadByteData(handle, MAG_Y_LSB));
//mag = -mag;
temp.push_back(mag);
}
max = *max_element(temp.begin(), temp.end());
min = *min_element(temp.begin(), temp.end());
temp.clear();
results.push_back(min);
results.push_back(max);
for (int i = 0; i < samples; i++){
mag = combineTwoRegister(i2cReadByteData(handle, MAG_Z_MSB), i2cReadByteData(handle, MAG_Z_LSB));
temp.push_back(mag);
}
max = *max_element(temp.begin(), temp.end());
min = *min_element(temp.begin(), temp.end());
temp.clear();
results.push_back(min);
results.push_back(max);
for (auto Element : results){
std::cout << Element << std::endl;
}
return results;
}
Here’s the code. I’ve read that I have to calibrate hard-iron distortion, so I made a function which reads 5000 samples in every direction (manually rotated LSM303D), than it applies correction to data. First I was doing it on my own, getting data, putting into file and drawing a “circle”. Next it calculate X and Y magnetic with pitch and roll included, than calc heading and applies declination. But it doesn’t work correctly. When the magnetometer lies on flat surface, the heading is quite good (however it can show ± 15 degrees difference while turning around 90 deg - I think it’s not enough hard-iron calibration), but when I rotate it around X or Y axis, heading differs by ~20-40 degrees. What did I do wrong? I’ve tried to put - in MagY and AccX and AccY axis (not sure if the device is not upside-down)