did a lot of thing lately

1 files changed, 0 insertions, 120 deletions

diff --git a/OLD_CODE_REMOVE_LATER/debug/prewk/IMU_comp/HeadingWithLSM6/HeadingWithLSM6.ino b/OLD_CODE_REMOVE_LATER/debug/prewk/IMU_comp/HeadingWithLSM6/HeadingWithLSM6.ino
deleted file mode 100644
index ac60515..0000000
--- a/OLD_CODE_REMOVE_LATER/debug/prewk/IMU_comp/HeadingWithLSM6/HeadingWithLSM6.ino
+++ /dev/null
@@ -1,120 +0,0 @@
-/*
-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});
-}