Skip to content

Instantly share code, notes, and snippets.

@srlm-io srlm-io/eCompass.ino
Last active Oct 3, 2017

Embed
What would you like to do?
LSM303DLHC eCompass implementation
#include <Wire.h>
#include <Adafruit_Sensor.h>
#include <Adafruit_LSM303_U.h>
#include <orientation.h>
// For the fabs function in fixedWidthPrint
#include <Math.h>
/* Assign a unique ID to this sensor at the same time */
Adafruit_LSM303_Accel_Unified accl = Adafruit_LSM303_Accel_Unified(54321);
Adafruit_LSM303_Mag_Unified magn = Adafruit_LSM303_Mag_Unified(12345);
void displaySensorDetails(void)
{
sensor_t sensor;
accl.getSensor(&sensor);
Serial.println("------------------------------------");
Serial.print ("Sensor: ");
Serial.println(sensor.name);
Serial.print ("Driver Ver: ");
Serial.println(sensor.version);
Serial.print ("Unique ID: ");
Serial.println(sensor.sensor_id);
Serial.print ("Max Value: ");
Serial.print(sensor.max_value);
Serial.println(" m/s^2");
Serial.print ("Min Value: ");
Serial.print(sensor.min_value);
Serial.println(" m/s^2");
Serial.print ("Resolution: ");
Serial.print(sensor.resolution);
Serial.println(" m/s^2");
Serial.println("------------------------------------");
Serial.println("");
delay(500);
}
void setup(void)
{
Serial.begin(9600);
Serial.println("Tilt Compensated Compass Test");
Serial.println("");
/* Initialise the sensor */
if(!accl.begin() || !magn.begin()) // also initalizes mag and accel sensors
{
/* There was a problem detecting the ADXL345 ... check your connections */
Serial.println("Ooops, no LSM303 detected ... Check your wiring!");
while(1);
}
/* Enable auto-gain */
magn.enableAutoRange(true);
/* Display some basic information on this sensor */
displaySensorDetails();
// Boston geomagnetic field
// Declination -14 degrees
// Inclunation -6.4 degrees
// Horizontal Intensity 20,000 nT
// North Component 19,000 nT north
// East Component 5,000 nT west
// Vertical Component 48,000 nT down
// Total Field 52,000 nT
float declination = -14;
// Boat
float hardiron_x = -9.715;
float hardiron_y = 2.77;
float hardiron_z = -2.955;
Orientation::setParameters(&accl, &magn, declination, hardiron_x, hardiron_y, hardiron_z);
}
void fixedWidthPrint(float value){
// Bug: one too many spaces when value === 10
int characters = 0;
// Count the number of digits before the decimal point
for(int i = 10000; i > 0; i /= 10){
if((int)fabs(value) >= i){
characters ++;
}
}
if(characters == 0){
characters = 1; // Minimum of 1 character if we print '0'
}
if(fabs(value) != value){ // Is it negative?
characters++;
}
for(int i = 6; i > characters; i--){
Serial.print(' ');
}
Serial.print(value, 2);
}
void loop(void)
{
float roll;
float pitch;
float yaw;
float heading;
Orientation::calculate(roll, pitch, yaw, heading);
Serial.print(" | roll:"); fixedWidthPrint(roll);
Serial.print(" | pitch:"); fixedWidthPrint(pitch);
Serial.print(" | yaw:"); fixedWidthPrint(yaw);
Serial.print(" | heading:"); fixedWidthPrint(heading);
Serial.print('\n');
}
set term png
set output "magn.png"
set terminal png size 1600,1600
print "##### magn x"
stats 'magn.tsv' using 1
print "##### magn y"
stats 'magn.tsv' using 2
print "##### magn z"
stats 'magn.tsv' using 3
set multiplot layout 2, 2
set xlabel "x"
set ylabel "y"
set size square
plot 'magn.tsv' using 1:2 with points title "x vs. y"
set nolabel
set xlabel "x"
set ylabel "z"
plot 'magn.tsv' using 1:3 with points title "x vs. z"
set xlabel "y"
set ylabel "z"
plot 'magn.tsv' using 2:3 with points title "y vs. z"
# 3D Plot
set nokey
set xlabel "x"
set ylabel "y"
set zlabel "z"
set view equal xyz
splot 'magn.tsv' with points
unset multiplot
#include <Wire.h>
#include <Adafruit_Sensor.h>
#include <Adafruit_LSM303_U.h>
/* Assign a unique ID to these sensors */
Adafruit_LSM303_Accel_Unified accel = Adafruit_LSM303_Accel_Unified(54321);
Adafruit_LSM303_Mag_Unified mag = Adafruit_LSM303_Mag_Unified(12345);
float AccelMinX, AccelMaxX;
float AccelMinY, AccelMaxY;
float AccelMinZ, AccelMaxZ;
float MagMinX, MagMaxX;
float MagMinY, MagMaxY;
float MagMinZ, MagMaxZ;
long lastDisplayTime;
void setup(void)
{
Serial.begin(9600);
Serial.println("LSM303 Calibration"); Serial.println("");
/* Initialise the accelerometer */
if(!accel.begin())
{
/* There was a problem detecting the ADXL345 ... check your connections */
Serial.println("Ooops, no LSM303 detected ... Check your wiring!");
while(1);
}
/* Initialise the magnetometer */
if(!mag.begin())
{
/* There was a problem detecting the LSM303 ... check your connections */
Serial.println("Ooops, no LSM303 detected ... Check your wiring!");
while(1);
}
lastDisplayTime = millis();
}
void loop(void)
{
/* Get a new sensor event */
sensors_event_t accelEvent;
sensors_event_t magEvent;
accel.getEvent(&accelEvent);
mag.getEvent(&magEvent);
if (accelEvent.acceleration.x < AccelMinX) AccelMinX = accelEvent.acceleration.x;
if (accelEvent.acceleration.x > AccelMaxX) AccelMaxX = accelEvent.acceleration.x;
if (accelEvent.acceleration.y < AccelMinY) AccelMinY = accelEvent.acceleration.y;
if (accelEvent.acceleration.y > AccelMaxY) AccelMaxY = accelEvent.acceleration.y;
if (accelEvent.acceleration.z < AccelMinZ) AccelMinZ = accelEvent.acceleration.z;
if (accelEvent.acceleration.z > AccelMaxZ) AccelMaxZ = accelEvent.acceleration.z;
if (magEvent.magnetic.x < MagMinX) MagMinX = magEvent.magnetic.x;
if (magEvent.magnetic.x > MagMaxX) MagMaxX = magEvent.magnetic.x;
if (magEvent.magnetic.y < MagMinY) MagMinY = magEvent.magnetic.y;
if (magEvent.magnetic.y > MagMaxY) MagMaxY = magEvent.magnetic.y;
if (magEvent.magnetic.z < MagMinZ) MagMinZ = magEvent.magnetic.z;
if (magEvent.magnetic.z > MagMaxZ) MagMaxZ = magEvent.magnetic.z;
if ((millis() - lastDisplayTime) > 1000) // display once/second
{
float hardiron_x = (MagMaxX + MagMinX) / 2;
float hardiron_y = (MagMaxY + MagMinY) / 2;
float hardiron_z = (MagMaxZ + MagMinZ) / 2;
Serial.print(" | hardiron_x: "); Serial.print(hardiron_x, 3); Serial.print(" | hardiron_y: "); Serial.print(hardiron_y, 3); Serial.print(" | hardiron_z:"); Serial.print(hardiron_z, 3);
Serial.print('\n');
lastDisplayTime = millis();
}
}
#include <Wire.h>
#include <Adafruit_Sensor.h>
#include <Adafruit_LSM303_U.h>
/* Assign a unique ID to these sensors */
Adafruit_LSM303_Accel_Unified accel = Adafruit_LSM303_Accel_Unified(54321);
Adafruit_LSM303_Mag_Unified mag = Adafruit_LSM303_Mag_Unified(12345);
float AccelMinX, AccelMaxX;
float AccelMinY, AccelMaxY;
float AccelMinZ, AccelMaxZ;
float MagMinX, MagMaxX;
float MagMinY, MagMaxY;
float MagMinZ, MagMaxZ;
long lastDisplayTime;
void setup(void)
{
Serial.begin(9600);
/* Initialise the accelerometer */
if(!accel.begin())
{
/* There was a problem detecting the ADXL345 ... check your connections */
Serial.println("Ooops, no LSM303 detected ... Check your wiring!");
while(1);
}
/* Initialise the magnetometer */
if(!mag.begin())
{
/* There was a problem detecting the LSM303 ... check your connections */
Serial.println("Ooops, no LSM303 detected ... Check your wiring!");
while(1);
}
lastDisplayTime = millis();
}
void loop(void)
{
/* Get a new sensor event */
sensors_event_t accelEvent;
sensors_event_t magEvent;
accel.getEvent(&accelEvent);
mag.getEvent(&magEvent);
float accl_x = accelEvent.acceleration.x;
float accl_y = accelEvent.acceleration.y;
float accl_z = accelEvent.acceleration.z;
float magn_x = magEvent.magnetic.x;
float magn_y = magEvent.magnetic.y;
float magn_z = magEvent.magnetic.z;
Serial.print(magn_x);
Serial.print("\t");
Serial.print(magn_y);
Serial.print("\t");
Serial.print(magn_z);
Serial.print("\n");
delay(10); // Limit to, at most, 100Hz.
}
#include <orientation.h>
#define RAD_CONV 57.2957 // 180/Pi, to convert radians to degrees
float Orientation::hardiron_x, Orientation::hardiron_y, Orientation::hardiron_z;
float Orientation::declination;
Adafruit_LSM303_Accel_Unified * Orientation::accl;
Adafruit_LSM303_Mag_Unified * Orientation::magn;
/** Use an eCompass algorithm to calculate orientation
*
* The algorithm is based on http://cache.freescale.com/files/sensors/doc/app_note/AN4248.pdf
*
* Hardiron calibration must be performed. The process is simple:
* 1. Mount the magnetometer in the location that you intend to use it at
* 2. Rotate the body through all possible orientations
* 3. Record the minimum and maximum for each axis of the magnetometer
* 4. Average the minumum and maximum for each axis. This will give you your hardiron x,y,z offsets.
*
* @param accl The LSM303 acceleration object
* @param magn The LSM303 magnetometer object
* @param hardiron_x
* @param hardiron_y
* @param hardiron_z
* @param roll The result roll in degrees
* @param pitch The result pitch in degrees
* @param yaw The result yaw in degrees
*/
void Orientation::calculate(float & roll, float & pitch, float & yaw, float & heading){
// Get a new sensor event
sensors_event_t event_accl;
sensors_event_t event_magn;
accl->getEvent(&event_accl);
magn->getEvent(&event_magn);
// Signs choosen so that, when axis is down, the value is + 1g
float accl_x = -event_accl.acceleration.x;
float accl_y = event_accl.acceleration.y;
float accl_z = event_accl.acceleration.z;
// Signs should be choosen so that, when the axis is down, the value is + positive.
// But that doesn't seem to work ?...
float magn_x = event_magn.magnetic.x - hardiron_x;
float magn_y = -event_magn.magnetic.y - hardiron_y;
float magn_z = -event_magn.magnetic.z - hardiron_z;
// Freescale solution
roll = atan2(accl_y, accl_z);
pitch = atan(-accl_x / (accl_y * sin(roll) + accl_z * cos(roll)));
float magn_fy_fs = magn_z * sin(roll) - magn_y*cos(roll);
float magn_fx_fs = magn_x * cos(pitch) + magn_y * sin(pitch) * sin(roll) + magn_z * sin(pitch) * cos(roll);
yaw = atan2(magn_fy_fs, magn_fx_fs);
roll = roll * RAD_CONV;
pitch = pitch * RAD_CONV;
yaw = yaw * RAD_CONV;
heading = yawToHeading(yaw);
}
/** Convert a yaw (-180 to 180) to a compass heading (0 to 360) with declination correction
*
* @param yaw The input yaw, in degrees -180 to 180
* @param declination the magnetic declination to adjust against, with sign. Note that this is time and location dependent.
* @return the corrected heading, in degrees 0 to 360
*/
float Orientation::yawToHeading(float yaw){
float heading = yaw + declination;
if (heading < 0.0){
heading += 360.0;
}
return heading;
}
void Orientation::setParameters(
Adafruit_LSM303_Accel_Unified * new_accl, Adafruit_LSM303_Mag_Unified * new_magn,
float new_declination,
float new_hardiron_x, float new_hardiron_y, float new_hardiron_z){
accl = new_accl;
magn = new_magn;
declination = new_declination;
hardiron_x = new_hardiron_x;
hardiron_y = new_hardiron_y;
hardiron_z = new_hardiron_z;
}
#include <Adafruit_Sensor.h>
#include <Adafruit_LSM303_U.h>
class Orientation {
public:
static void calculate(float & roll, float & pitch, float & yaw, float & heading);
static float yawToHeading(float yaw);
static void setParameters(
Adafruit_LSM303_Accel_Unified * accl, Adafruit_LSM303_Mag_Unified * magn,
float declination,
float hardiron_x, float hardiron_y, float hardiron_z);
static Adafruit_LSM303_Accel_Unified * accl;
static Adafruit_LSM303_Mag_Unified * magn;
static float declination;
static float hardiron_x, hardiron_y, hardiron_z;
};
#include <iostream>
#include <fstream>
using namespace::std;
void compare(float value, float & min, float & max){
if(value < min){
min = value;
}
if(value > max){
max = value;
}
}
void printResult(string axis, float min, float max){
cout << axis
<< ": min: " << min
<< ", max: " << max
<< ", range: " << (max - min)
<< ", middle: " << ((max - min)/2 + min)
<< "\n";
}
int main()
{
cout << "Hello, world!\n";
ifstream infile("magn.tsv");
float x, y, z;
float min_x, max_x;
float min_y, max_y;
float min_z, max_z;
int line = 0;
while(infile >> x >> y >> z){
line ++;
if(z < -1000){
cout << "line: " << line << "\n";
}
compare(x, min_x, max_x);
compare(y, min_y, max_y);
compare(z, min_z, max_z);
}
printResult("X", min_x, max_x);
printResult("Y", min_y, max_y);
printResult("Z", min_z, max_z);
}
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
You can’t perform that action at this time.