Bump version to 2.4.0 in library.properties

clang format

.github/workflows/githubci.yml

@@ -7,11 +7,11 @@ jobs:
     runs-on: ubuntu-latest
     
     steps:
-    - uses: actions/setup-python@v1
+    - uses: actions/setup-python@v4
       with:
         python-version: '3.x'
-    - uses: actions/checkout@v2
-    - uses: actions/checkout@v2
+    - uses: actions/checkout@v3
+    - uses: actions/checkout@v3
       with:
          repository: adafruit/ci-arduino
          path: ci

examples/calibrated_orientation/LSM6DS_LIS3MDL.h

@@ -2,8 +2,12 @@
 Adafruit_LIS3MDL lis3mdl;
 
 // Can change this to be LSM6DSOX or whatever ya like
+// For (older) Feather Sense with LSM6DS33, use this:
 #include <Adafruit_LSM6DS33.h>
 Adafruit_LSM6DS33 lsm6ds;
+// For (newer) Feather Sense with LSM6DS3TR-C, use this:
+// #include <Adafruit_LSM6DS3TRC.h>
+// Adafruit_LSM6DS3TRC lsm6ds;
 
 bool init_sensors(void) {
   if (!lsm6ds.begin_I2C() || !lis3mdl.begin_I2C()) {

examples/calibrated_orientation/LSM9DS.h

@@ -2,7 +2,7 @@
 Adafruit_LSM9DS1 lsm9ds = Adafruit_LSM9DS1();
 
 // Or if you have the older LSM9DS0
-//#include <Adafruit_LSM9DS0.h>
+// #include <Adafruit_LSM9DS0.h>
 // Adafruit_LSM9DS0 lsm9ds = Adafruit_LSM9DS0();
 
 bool init_sensors(void) {

examples/calibrated_orientation/calibrated_orientation.ino

@@ -13,7 +13,7 @@
 Adafruit_Sensor *accelerometer, *gyroscope, *magnetometer;
 
 // uncomment one combo 9-DoF!
-#include "LSM6DS_LIS3MDL.h"  // can adjust to LSM6DS33, LSM6DS3U, LSM6DSOX...
+#include "LSM6DS_LIS3MDL.h"  // see the the LSM6DS_LIS3MDL file in this project to change board to LSM6DS33, LSM6DS3U, LSM6DSOX, etc
 //#include "LSM9DS.h"           // LSM9DS1 or LSM9DS0
 //#include "NXP_FXOS_FXAS.h"  // NXP 9-DoF breakout
 

examples/calibration/LSM6DS_LIS3MDL.h

@@ -2,8 +2,12 @@
 Adafruit_LIS3MDL lis3mdl;
 
 // Can change this to be LSM6DS33 or whatever ya like
+// For (older) Feather Sense with LSM6DS33, use this:
 #include <Adafruit_LSM6DS33.h>
 Adafruit_LSM6DS33 lsm6ds;
+// For (newer) Feather Sense with LSM6DS3TR-C, use this:
+// #include <Adafruit_LSM6DS3TRC.h>
+// Adafruit_LSM6DS3TRC lsm6ds;
 
 bool init_sensors(void) {
   if (!lsm6ds.begin_I2C() || !lis3mdl.begin_I2C()) {

examples/calibration/LSM9DS.h

@@ -1,5 +1,5 @@
-//#include <Adafruit_LSM9DS1.h>
-// Adafruit_LSM9DS1 lsm9ds = Adafruit_LSM9DS1();
+// #include <Adafruit_LSM9DS1.h>
+//  Adafruit_LSM9DS1 lsm9ds = Adafruit_LSM9DS1();
 
 // Or if you have the older LSM9DS0
 #include <Adafruit_LSM9DS0.h>

examples/deprecated/ahrs_10dof/ahrs_10dof.ino

@@ -1,13 +1,14 @@
 #include <Wire.h>
 #include <Adafruit_Sensor.h>
-#include <Adafruit_LSM303_U.h>
+#include <Adafruit_LSM303_Accel.h>
+#include <Adafruit_LSM303DLH_Mag.h>
 #include <Adafruit_BMP085_U.h>
 #include <Adafruit_Simple_AHRS.h>
 
 // Create sensor instances.
-Adafruit_LSM303_Accel_Unified accel(30301);
-Adafruit_LSM303_Mag_Unified   mag(30302);
-Adafruit_BMP085_Unified       bmp(18001);
+Adafruit_LSM303_Accel_Unified  accel(30301);
+Adafruit_LSM303DLH_Mag_Unified mag(30302);
+Adafruit_BMP085_Unified        bmp(18001);
 
 // Create simple AHRS algorithm using the above sensors.
 Adafruit_Simple_AHRS          ahrs(&accel, &mag);

examples/deprecated/ahrs_9dof/ahrs_9dof.ino

@@ -1,11 +1,12 @@
 #include <Wire.h>
 #include <Adafruit_Sensor.h>
-#include <Adafruit_LSM303_U.h>
+#include <Adafruit_LSM303_Accel.h>
+#include <Adafruit_LSM303DLH_Mag.h>
 #include <Adafruit_Simple_AHRS.h>
 
 // Create sensor instances.
-Adafruit_LSM303_Accel_Unified accel(30301);
-Adafruit_LSM303_Mag_Unified   mag(30302);
+Adafruit_LSM303_Accel_Unified  accel(30301);
+Adafruit_LSM303DLH_Mag_Unified mag(30302);
 
 // Create simple AHRS algorithm using the above sensors.
 Adafruit_Simple_AHRS          ahrs(&accel, &mag);

library.properties

@@ -1,5 +1,5 @@
 name=Adafruit AHRS
-version=2.3.1
+version=2.4.0
 author=Adafruit
 maintainer=Adafruit <info@adafruit.com>
 sentence=AHRS (Altitude and Heading Reference System) for various Adafruit motion sensors
@@ -7,4 +7,4 @@ paragraph=Includes motion calibration example sketches, as well as calibration o
 category=Sensors
 url=https://github.com/adafruit/Adafruit_AHRS
 architectures=*
-depends=Adafruit Unified Sensor, Adafruit LSM6DS, Adafruit LIS3MDL, Adafruit FXOS8700, Adafruit FXAS21002C, Adafruit LSM9DS1 Library, Adafruit LSM9DS0 Library, Adafruit LSM303DLHC, Adafruit BMP085 Unified, Adafruit BluefruitLE nRF51, SdFat - Adafruit Fork, ArduinoJson, Adafruit SPIFlash, Adafruit Sensor Calibration
+depends=Adafruit Unified Sensor, Adafruit LSM6DS, Adafruit LIS3MDL, Adafruit FXOS8700, Adafruit FXAS21002C, Adafruit LSM9DS1 Library, Adafruit LSM9DS0 Library, Adafruit BMP085 Unified, Adafruit BluefruitLE nRF51, SdFat - Adafruit Fork, ArduinoJson, Adafruit SPIFlash, Adafruit Sensor Calibration, Adafruit LSM303 Accel, Adafruit LSM303DLH Mag

src/Adafruit_AHRS_FusionInterface.h

@@ -90,6 +90,16 @@ public:
   virtual float getYaw() = 0;
   virtual void getQuaternion(float *w, float *x, float *y, float *z) = 0;
   virtual void setQuaternion(float w, float x, float y, float z) = 0;
+
+  /**************************************************************************/
+  /*!
+   * @brief Gets the current gravity vector of the sensor.
+   *
+   * @param x A float pointer to write the gravity vector x component to. In g.
+   * @param y A float pointer to write the gravity vector y component to. In g.
+   * @param z A float pointer to write the gravity vector z component to. In g.
+   */
+  virtual void getGravityVector(float *x, float *y, float *z) = 0;
 };
 
 #endif /* ADAFRUIT_AHRS_FUSIONINTERFACE_H_ */

src/Adafruit_AHRS_Madgwick.cpp

@@ -43,7 +43,7 @@ Adafruit_Madgwick::Adafruit_Madgwick(float gain) {
   q2 = 0.0f;
   q3 = 0.0f;
   invSampleFreq = 1.0f / sampleFreqDef;
-  anglesComputed = 0;
+  anglesComputed = false;
 }
 
 void Adafruit_Madgwick::update(float gx, float gy, float gz, float ax, float ay,
@@ -282,13 +282,14 @@ void Adafruit_Madgwick::updateIMU(float gx, float gy, float gz, float ax,
 
 float Adafruit_Madgwick::invSqrt(float x) {
   float halfx = 0.5f * x;
-  float y = x;
-  long i = *(long *)&y;
-  i = 0x5f3759df - (i >> 1);
-  y = *(float *)&i;
-  y = y * (1.5f - (halfx * y * y));
-  y = y * (1.5f - (halfx * y * y));
-  return y;
+  union {
+    float f;
+    long i;
+  } conv = {x};
+  conv.i = 0x5f3759df - (conv.i >> 1);
+  conv.f *= 1.5f - (halfx * conv.f * conv.f);
+  conv.f *= 1.5f - (halfx * conv.f * conv.f);
+  return conv.f;
 }
 
 //-------------------------------------------------------------------------------------------
@@ -297,5 +298,8 @@ void Adafruit_Madgwick::computeAngles() {
   roll = atan2f(q0 * q1 + q2 * q3, 0.5f - q1 * q1 - q2 * q2);
   pitch = asinf(-2.0f * (q1 * q3 - q0 * q2));
   yaw = atan2f(q1 * q2 + q0 * q3, 0.5f - q2 * q2 - q3 * q3);
+  grav[0] = 2.0f * (q1 * q3 - q0 * q2);
+  grav[1] = 2.0f * (q0 * q1 + q2 * q3);
+  grav[2] = 2.0f * (q0 * q0 - 0.5f + q3 * q3);
   anglesComputed = 1;
 }

src/Adafruit_AHRS_Madgwick.h

@@ -31,10 +31,9 @@ private:
   float q2;
   float q3; // quaternion of sensor frame relative to auxiliary frame
   float invSampleFreq;
-  float roll;
-  float pitch;
-  float yaw;
-  char anglesComputed;
+  float roll, pitch, yaw;
+  float grav[3];
+  bool anglesComputed = false;
   void computeAngles();
 
   //-------------------------------------------------------------------------------------------
@@ -98,5 +97,12 @@ public:
     q2 = y;
     q3 = z;
   }
+  void getGravityVector(float *x, float *y, float *z) {
+    if (!anglesComputed)
+      computeAngles();
+    *x = grav[0];
+    *y = grav[1];
+    *z = grav[2];
+  }
 };
 #endif

src/Adafruit_AHRS_Mahony.cpp

@@ -49,7 +49,7 @@ Adafruit_Mahony::Adafruit_Mahony(float prop_gain, float int_gain) {
   integralFBx = 0.0f;
   integralFBy = 0.0f;
   integralFBz = 0.0f;
-  anglesComputed = 0;
+  anglesComputed = false;
   invSampleFreq = 1.0f / DEFAULT_SAMPLE_FREQ;
 }
 
@@ -261,13 +261,14 @@ void Adafruit_Mahony::updateIMU(float gx, float gy, float gz, float ax,
 
 float Adafruit_Mahony::invSqrt(float x) {
   float halfx = 0.5f * x;
-  float y = x;
-  long i = *(long *)&y;
-  i = 0x5f3759df - (i >> 1);
-  y = *(float *)&i;
-  y = y * (1.5f - (halfx * y * y));
-  y = y * (1.5f - (halfx * y * y));
-  return y;
+  union {
+    float f;
+    long i;
+  } conv = {x};
+  conv.i = 0x5f3759df - (conv.i >> 1);
+  conv.f *= 1.5f - (halfx * conv.f * conv.f);
+  conv.f *= 1.5f - (halfx * conv.f * conv.f);
+  return conv.f;
 }
 
 //-------------------------------------------------------------------------------------------
@@ -276,6 +277,9 @@ void Adafruit_Mahony::computeAngles() {
   roll = atan2f(q0 * q1 + q2 * q3, 0.5f - q1 * q1 - q2 * q2);
   pitch = asinf(-2.0f * (q1 * q3 - q0 * q2));
   yaw = atan2f(q1 * q2 + q0 * q3, 0.5f - q2 * q2 - q3 * q3);
+  grav[0] = 2.0f * (q1 * q3 - q0 * q2);
+  grav[1] = 2.0f * (q0 * q1 + q2 * q3);
+  grav[2] = 2.0f * (q0 * q0 - 0.5f + q3 * q3);
   anglesComputed = 1;
 }
 

src/Adafruit_AHRS_Mahony.h

@@ -29,7 +29,8 @@ private:
       integralFBz; // integral error terms scaled by Ki
   float invSampleFreq;
   float roll, pitch, yaw;
-  char anglesComputed;
+  float grav[3];
+  bool anglesComputed = false;
   static float invSqrt(float x);
   void computeAngles();
 
@@ -87,13 +88,19 @@ public:
     *y = q2;
     *z = q3;
   }
-
   void setQuaternion(float w, float x, float y, float z) {
     q0 = w;
     q1 = x;
     q2 = y;
     q3 = z;
   }
+  void getGravityVector(float *x, float *y, float *z) {
+    if (!anglesComputed)
+      computeAngles();
+    *x = grav[0];
+    *y = grav[1];
+    *z = grav[2];
+  }
 };
 
 #endif

src/Adafruit_AHRS_NXPFusion.cpp

@@ -633,7 +633,7 @@ void Adafruit_NXPSensorFusion::update(float gx, float gy, float gz, float ax,
       mGl[X] = DEFAULTB * fcosdelta; // TODO: MagCal->B
       mGl[Z] = DEFAULTB * fsindelta;
     } // end hyp == 0.0F
-  }   // end ValidMagCal
+  } // end ValidMagCal
 
   // *********************************************************************************
   // compute the a posteriori Euler angles from the orientation matrix

src/Adafruit_AHRS_NXPFusion.h

@@ -116,12 +116,12 @@ public:
   /*!
    * @brief Get the gravity vector from the gyroscope values.
    *
-   * @param x The pointer to write the gravity vector x axis to. In g.
-   * @param y The pointer to write the gravity vector y axis to. In g.
-   * @param z The pointer to write the gravity vector z axis to. In g.
+   * @param x A float pointer to write the gravity vector x component to. In g.
+   * @param y A float pointer to write the gravity vector y component to. In g.
+   * @param z A float pointer to write the gravity vector z component to. In g.
    */
   /**************************************************************************/
-  void getGravityVector(float *x, float *y, float *z) const {
+  void getGravityVector(float *x, float *y, float *z) {
     *x = gSeGyMi[0];
     *y = gSeGyMi[1];
     *z = gSeGyMi[2];

src/Adafruit_AHRS_NXPmatrix.c

@@ -296,9 +296,9 @@ void eigencompute(float A[][10], float eigval[], float eigvec[][10], int8_t n) {
               A[ic][j] = A[ic][j] + sinphi * (ftmp - tanhalfphi * A[ic][j]);
             }
           } // end of test for matrix element already zero
-        }   // end of loop over columns
-      }     // end of loop over rows
-    }       // end of test for non-zero residue
+        } // end of loop over columns
+      } // end of loop over rows
+    } // end of test for non-zero residue
   } while ((residue > 0.0F) && (ctr++ < NITERATIONS)); // end of main loop
 }