Guide to gyro and accelerometer with Arduino including Kalman filtering

Dear Thomas, Lauszus,

Thanks for the time you took to reply.

I read the article and searched for a calibration programm related to the LSM303. I found this program:

#include <Wire.h>
#include <LSM303.h>

LSM303 compass;
LSM303::vector running_min = {2047, 2047, 2047}, running_max = {-2048, -2048, -2048};

// temp
float x_min; float x_max; float x_cur;
float y_min; float y_max; float y_cur;
float z_min; float z_max; float z_cur;

void setup() {
  
  Serial.begin(9600);
  Wire.begin();
  compass.init();
  compass.enableDefault();
 
}

void loop() {  
  compass.read();
  
  running_min.x = min(running_min.x, compass.m.x);
  running_min.y = min(running_min.y, compass.m.y);
  running_min.z = min(running_min.z, compass.m.z);

  running_max.x = max(running_max.x, compass.m.x);
  running_max.y = max(running_max.y, compass.m.y);
  running_max.z = max(running_max.z, compass.m.z);
  
  Serial.print("M min ");
  Serial.print("X: ");
  Serial.print((int)running_min.x);
  Serial.print(" Y: ");
  Serial.print((int)running_min.y);
  Serial.print(" Z: ");
  Serial.print((int)running_min.z);

  Serial.print(" M max ");  
  Serial.print("X: ");
  Serial.print((int)running_max.x);
  Serial.print(" Y: ");
  Serial.print((int)running_max.y);
  Serial.print(" Z: ");
  Serial.println((int)running_max.z);
  
  delay(100);
}

After running the programm and looking at the serial output, I started moving the board in all sorts of direction, and after some time I got a stable output:

/* Output of Calibrate.ino
M min X: -548 Y: -378 Z: -547 M max X: 248 Y: 413 Z: 314
M min X: -548 Y: -378 Z: -547 M max X: 248 Y: 413 Z: 314
M min X: -548 Y: -378 Z: -547 M max X: 248 Y: 413 Z: 314
*/

These values I entered in the programm "Heading.ino"

#include <Wire.h>
#include <LSM303.h>

LSM303 compass;

void setup() {
  Serial.begin(9600);
  Wire.begin();
  compass.init();
  compass.enableDefault();
  
  // Calibration values. Use the Calibrate example program to get the values for
  // your compass.
  compass.m_min.x = -548; compass.m_min.y = -378; compass.m_min.z = -547;
  compass.m_max.x = 248; compass.m_max.y = 413; compass.m_max.z = 314;
  
  /* Output van Calibrate
  M min X: -548 Y: -378 Z: -547 M max X: 248 Y: 413 Z: 314
  M min X: -548 Y: -378 Z: -547 M max X: 248 Y: 413 Z: 314
  M min X: -548 Y: -378 Z: -547 M max X: 248 Y: 413 Z: 314
  */
}

/* Oorspronkelijke code
void loop() {
  compass.read();
  int heading = compass.heading((LSM303::vector){0,-1,0});
  Serial.println(heading);
  delay(500);
} 
*/

/*----------------------------------------------------------------------------
        1   2   3 4   5 6
        |   |   | |   | |
 $--HDG,x.x,x.x,a,x.x,a*hh<CR><LF>
------------------------------------------------------------------------------

Field Number: 
1. Magnetic Sensor heading in degrees
2. Magnetic Deviation, degrees
3. Magnetic Deviation direction, E = Easterly, W = Westerly
4. Magnetic Variation degrees
5. Magnetic Variation direction, E = Easterly, W = Westerly
6. Checksum */

char nmeastr[17]; // HCHDG,000.00,,,,*
int checksum;
char hs[6]; // 000.00

void loop() {
  strcpy(nmeastr,"HCHDG,");
  
  //dtostrf(FLOAT,WIDTH,PRECSISION,BUFFER);
  compass.read();
  dtostrf(compass.heading((LSM303::vector){0,-1,0}), 5, 0, hs);
  //Serial.print("hs ");
  //Serial.print(hs);
  //Serial.println();  
  
  strcat(nmeastr,hs);
  strcat(nmeastr,",,,,");
  
  //add a checksum
  checksum=0;
  for (int n=0; n < strlen(nmeastr); n++) {
    checksum ^= nmeastr[n];
  }
  
  Serial.print("$");
  Serial.print(nmeastr);
  Serial.print("*");
  Serial.print(checksum, HEX);
  Serial.println();
  
  /* 5hz == 200 */
  delay(500);
}

While running this programm, serial output showed the heading as NMEA-string. Having it away from all sorts of electroncs and put a normal (old fashion :wink: ) compass pointing in the same directoin, and laying flat on the floor,, the output showed:

$HCHDG, 0,,,,*5C
$HCHDG, 2,,,,*5E
$HCHDG, 5,,,,*59
$HCHDG, 1,,,,*5D
$HCHDG, 3,,,,*5F
$HCHDG, 1,,,,*5D
$HCHDG, 2,,,,*5E
$HCHDG, 4,,,,*58
$HCHDG, 1,,,,*5D
$HCHDG, 2,,,,*5E
$HCHDG, 2,,,,*5E
$HCHDG, 0,,,,*5C
$HCHDG, 3,,,,*5F
$HCHDG, 1,,,,*5D
$HCHDG, 2,,,,*5E
$HCHDG, 3,,,,*5F
$HCHDG, 3,,,,*5F
$HCHDG, 3,,,,*5F
$HCHDG, 3,,,,*5F
$HCHDG, 2,,,,*5E
$HCHDG, 4,,,,*58
$HCHDG, 4,,,,*58
$HCHDG, 5,,,,*59

Allthough not very accurated, it points north with some variance. Not too bad, but room for improvement, I think.

As soon as I start turning the lsm303 up and down , still pointing in the same direction, I would expect to see the same heading however the output shows:

$HCHDG, 14,,,,*49
$HCHDG, 14,,,,*49
$HCHDG, 16,,,,*4B
$HCHDG, 12,,,,*4F
$HCHDG, 10,,,,*4D
$HCHDG, 6,,,,*5A
$HCHDG, 5,,,,*59
$HCHDG, 6,,,,*5A
$HCHDG, 10,,,,*4D
$HCHDG, 7,,,,*5B
$HCHDG, 9,,,,*55
$HCHDG, 8,,,,*54
$HCHDG, 9,,,,*55
$HCHDG, 12,,,,*4F
$HCHDG, 13,,,,*4E
$HCHDG, 11,,,,*4C
$HCHDG, 10,,,,*4D
$HCHDG, 11,,,,*4C

IMHO, this heading info is too inaccurate to use as a compass. The autopilot will move the boat from left to right, like a drunken sailor :wink:

I'm not sure wether this is part of the lsm303 (in that case it is not a sufficiënt solution for a compass for an auto pilot) or filtering or something else is required to get a more stable output.

Many thanks, George