1 files changed, 120 insertions, 0 deletions
diff --git a/CODE/debug/IMU_comp/HeadingWithLSM6/HeadingWithLSM6.ino b/CODE/debug/IMU_comp/HeadingWithLSM6/HeadingWithLSM6.ino
new file mode 100644
index 0000000..ac60515
--- /dev/null
+++ b/CODE/debug/IMU_comp/HeadingWithLSM6/HeadingWithLSM6.ino
@@ -0,0 +1,120 @@
+/*
+This sketch combines magnetometer readings from an LIS3MDL and accelerometer
+readings from an LSM6 to calculate a tilt-compensated magnetic heading. It
+requires Pololu's LSM6 Arduino library to be installed:
+
+https://github.com/pololu/lsm6-arduino
+
+This program can be used with a board that includes both sensors, like the
+Pololu MinIMU-9 v5 and AltIMU-10 v5, or with separate carrier boards for the two
+sensors, both connected to the same I2C bus. If you are using separate boards,
+make sure the axes are oriented the same way on both (i.e. the X, Y, and Z axes
+of both boards should point in the same direction, and the surfaces of the
+boards should be as close to parallel as possible).
+*/
+
+#include <Wire.h>
+#include <LIS3MDL.h>
+#include <LSM6.h>
+
+LIS3MDL mag;
+LSM6 imu;
+
+/*
+Calibration values; the default values of +/-32767 for each axis
+lead to an assumed magnetometer bias of 0. Use the Calibrate example
+program to determine appropriate values for your particular unit.
+*/
+LIS3MDL::vector<int16_t> m_min = {+5862, +2474, -6186};
+LIS3MDL::vector<int16_t> m_max = {+6610, +2849, -5547};
+
+void setup()
+{
+  Serial.begin(9600);
+  Wire.begin();
+
+  if (!mag.init())
+  {
+    Serial.println("Failed to detect and initialize LIS3MDL magnetometer!");
+    while (1);
+  }
+  mag.enableDefault();
+
+  if (!imu.init())
+  {
+    Serial.println("Failed to detect and initialize LSM6 IMU!");
+    while (1);
+  }
+  imu.enableDefault();
+}
+
+void loop()
+{
+  mag.read();
+  imu.read();
+
+  /*
+  When given no arguments, the heading() function returns the angular
+  difference in the horizontal plane between a default vector (the
+  +X axis) and north, in degrees.
+  */
+  float heading = computeHeading();
+
+  /*
+  To use a different vector as a reference, use the version of
+  computeHeading() that takes a vector argument; for example, call it like this
+  to use the -Z axis as a reference:
+  */
+  //float heading = computeHeading((LIS3MDL::vector<int>){0, 0, -1});
+
+  Serial.println(heading);
+  delay(100);
+}
+
+/*
+Returns the angular difference in the horizontal plane between the
+"from" vector and north, in degrees.
+
+Description of heading algorithm:
+Shift and scale the magnetic reading based on calibration data to find
+the North vector. Use the acceleration readings to determine the Up
+vector (gravity is measured as an upward acceleration). The cross
+product of North and Up vectors is East. The vectors East and North
+form a basis for the horizontal plane. The From vector is projected
+into the horizontal plane and the angle between the projected vector
+and horizontal north is returned.
+*/
+template <typename T> float computeHeading(LIS3MDL::vector<T> from)
+{
+  LIS3MDL::vector<int32_t> temp_m = {mag.m.x, mag.m.y, mag.m.z};
+
+  // copy acceleration readings from LSM6::vector into an LIS3MDL::vector
+  LIS3MDL::vector<int16_t> a = {imu.a.x, imu.a.y, imu.a.z};
+
+  // subtract offset (average of min and max) from magnetometer readings
+  temp_m.x -= ((int32_t)m_min.x + m_max.x) / 2;
+  temp_m.y -= ((int32_t)m_min.y + m_max.y) / 2;
+  temp_m.z -= ((int32_t)m_min.z + m_max.z) / 2;
+
+  // compute E and N
+  LIS3MDL::vector<float> E;
+  LIS3MDL::vector<float> N;
+  LIS3MDL::vector_cross(&temp_m, &a, &E);
+  LIS3MDL::vector_normalize(&E);
+  LIS3MDL::vector_cross(&a, &E, &N);
+  LIS3MDL::vector_normalize(&N);
+
+  // compute heading
+  float heading = atan2(LIS3MDL::vector_dot(&E, &from), LIS3MDL::vector_dot(&N, &from)) * 180 / PI;
+  if (heading < 0) heading += 360;
+  return heading;
+}
+
+/*
+Returns the angular difference in the horizontal plane between a
+default vector (the +X axis) and north, in degrees.
+*/
+float computeHeading()
+{
+  return computeHeading((LIS3MDL::vector<int>){1, 0, 0});
+}