downgrade to v1.0.0

README.md

@@ -21,7 +21,3 @@ Since a 6-axis IMU has no absolute reference for heading there is a function to
 For more information on the Mahony filter see these links:
 - [IMU Data Fusing: Complementary, Kalman, and Mahony Filter](http://www.olliw.eu/2013/imu-data-fusing/#chapter23)
 - [Mahony Filter](https://nitinjsanket.github.io/tutorials/attitudeest/mahony)
-
-## Dependencies
-
-This library depends on the [vector_datatype](https://github.com/RCmags/vector_datatype) library.

examples/heading/heading.ino

@@ -1,41 +1,80 @@
-/*
- This sketch shows to perform vector products and rotate heading (yaw angle) of the estimated orientation.
- */
-
-#include <imuFilter.h>
-#include <basicMPU6050.h>       // Library for IMU sensor. See this link: https://github.com/RCmags/basicMPU6050
-
-// Sensor fusion
-constexpr float GAIN = 0.1;     // Fusion gain, value between 0 and 1 - Determines response of heading correction with respect to gravity.
-imuFilter <&GAIN> fusion;
-
-// Imu sensor
-basicMPU6050<> imu;
-
-void setup() {
-  // Initialize filter: 
-  fusion.setup( imu.ax(), imu.ay(), imu.az() );     
-
-  // Calibrate imu
-  imu.setup();
-  imu.setBias();
-
-  Serial.begin(38400);
-
-  // Rotate heading:
-  float angle = 45 * DEG_TO_RAD;                // angle in radians to rotate heading about z-axis
-  fusion.rotateHeading( angle, LARGE_ANGLE );   // Can choose LARGE_ANGLE or SMALL_ANGLE approximation
-}
-
-void loop() {  
-  // Update filter:
-  fusion.update( imu.gx(), imu.gy(), imu.gz(), imu.ax(), imu.ay(), imu.az() );    
-
-  // Display angles:
-  Serial.print( fusion.pitch() );
-  Serial.print( " " );
-  Serial.print( fusion.yaw() );
-  Serial.print( " " );
-  Serial.print( fusion.roll() );
-  Serial.println();
-}
+/*
+ This sketch shows to perform vector products and rotate heading (yaw angle) of the estimated orientation.
+ */
+
+#include <imuFilter.h>
+#include <basicMPU6050.h>       // Library for IMU sensor. See this link: https://github.com/RCmags/basicMPU6050
+
+// Sensor fusion
+constexpr float GAIN = 0.1;     // Fusion gain, value between 0 and 1 - Determines response of heading correction with respect to gravity.
+imuFilter <&GAIN> fusion;
+
+// Imu sensor
+basicMPU6050<> imu;
+
+// Display function:
+void printVector( float r[] ) {
+  Serial.print( r[0], 2 );
+  Serial.print( "," );
+  Serial.print( r[1], 2 );
+  Serial.print( "," );
+  Serial.print( r[2], 2 );
+  Serial.println();
+}
+
+void setup() {
+  // Initialize filter: 
+  fusion.setup( imu.ax(), imu.ay(), imu.az() );     
+
+  // Calibrate imu
+  imu.setup();
+  imu.setBias();
+
+  Serial.begin(38400);
+
+  // Vector operations:
+  float v1[] = { 3, 1, -1 };                    // Input Vector 
+  float axis_y[3], axis_z[3];
+
+  fusion.getYaxis( true, axis_y );              // Vectors to operate on [global axes] 
+  fusion.getZaxis( true, axis_z );              
+
+  fusion.crossProduct( v1, axis_y );            // Cross product: V = V cross R ; Output is stored in V 
+  float v2[] = { v1[0], v1[1], v1[2] };         // Store product
+
+  fusion.normalizeVector( v1 );                 // Norm: V = V/|V| ; Output is stored in V 
+
+  float dot = fusion.dotProduct( v1, axis_z );  // Dot product: Input order does not matter   
+
+  // Rotate heading:
+  #define SMALL_ANG false  
+                          // Small angle approximation = true 
+                          // Exact angle rotation = false 
+  fusion.rotateHeading( SMALL_ANG, dot );
+
+  // Display results:
+  Serial.print( "y = " ); 
+  printVector( axis_y );
+  Serial.print( "v2 = " ); 
+  printVector( v2 );
+  Serial.print( "v1 = " ); 
+  printVector( v1 );
+  Serial.print( "dot = ");
+  Serial.println( dot );
+
+  // Wait for output to be read
+  delay(10000);
+}
+
+void loop() {  
+  // Update filter:
+  fusion.update( imu.gx(), imu.gy(), imu.gz(), imu.ax(), imu.ay(), imu.az() );    
+
+  // Display angles:
+  Serial.print( fusion.pitch() );
+  Serial.print( " " );
+  Serial.print( fusion.yaw() );
+  Serial.print( " " );
+  Serial.print( fusion.roll() );
+  Serial.println();
+}

examples/output/output.ino

@@ -1,52 +1,52 @@
-/*
- This sketch shows to initialize the filter and update it with the imu output. It also shows how to get the euler angles of the estimated heading orientation.
- */
-
-#include <imuFilter.h>
-#include <basicMPU6050.h>       // Library for IMU sensor. See this link: https://github.com/RCmags/basicMPU6050
-
-// Sensor fusion
-constexpr float GAIN = 0.1;     // Fusion gain, value between 0 and 1 - Determines orientation correction with respect to gravity vector. 
-imuFilter <&GAIN> fusion;       // If set to 1 the gyroscope is dissabled. If set to 0 the accelerometer is dissabled (equivant to gyro-only).
-
-// Imu sensor
-basicMPU6050<> imu;
-
-// Enable sensor fusion. Setting to "true" enables gravity correction.
-#define FUSION  false
-
-void setup() {
-   #if FUSION
-    // Set quaternion with gravity vector
-    fusion.setup( imu.ax(), imu.ay(), imu.az() );     
-  #else 
-    // Just use gyro
-    fusion.setup();                                   
-  #endif
-
-  // Calibrate imu
-  imu.setup();
-  imu.setBias();
-
-  Serial.begin(38400);
-}
-
-void loop() {
-  // Update filter:
-
-  #if FUSION
-    //Fuse gyro and accelerometer
-    fusion.update( imu.gx(), imu.gy(), imu.gz(), imu.ax(), imu.ay(), imu.az() );    
-  #else
-    // Only use gyroscope
-    fusion.update( imu.gx(), imu.gy(), imu.gz() );
-  #endif
-
-  // Display angles:
-  Serial.print( fusion.pitch() );
-  Serial.print( " " );
-  Serial.print( fusion.yaw() );
-  Serial.print( " " );
-  Serial.print( fusion.roll() );
-  Serial.println(); 
-}
+/*
+ This sketch shows to initialize the filter and update it with the imu output. It also shows how to get the euler angles of the estimated heading orientation.
+ */
+
+#include <imuFilter.h>
+#include <basicMPU6050.h>       // Library for IMU sensor. See this link: https://github.com/RCmags/basicMPU6050
+
+// Sensor fusion
+constexpr float GAIN = 0.1;     // Fusion gain, value between 0 and 1 - Determines heading correction with respect to gravity vector. 
+imuFilter <&GAIN> fusion;
+
+// Imu sensor
+basicMPU6050<> imu;
+
+// Enable sensor fusion [ 1 = yes; 0 = no]
+#define FUSION 1
+
+void setup() {
+   #if FUSION
+    // Set quaternion with gravity vector
+    fusion.setup( imu.ax(), imu.ay(), imu.az() );     
+  #else 
+    // Just use gyro
+    fusion.setup();                                   
+  #endif
+
+  // Calibrate imu
+  imu.setup();
+  imu.setBias();
+
+  Serial.begin(38400);
+}
+
+void loop() {
+  // Update filter:
+
+  #if FUSION
+    //Fuse gyro and accelerometer
+    fusion.update( imu.gx(), imu.gy(), imu.gz(), imu.ax(), imu.ay(), imu.az() );    
+  #else
+    // Only use gyroscope
+    fusion.update( imu.gx(), imu.gy(), imu.gz() );
+  #endif
+
+  // Display angles:
+  Serial.print( fusion.pitch() );
+  Serial.print( " " );
+  Serial.print( fusion.yaw() );
+  Serial.print( " " );
+  Serial.print( fusion.roll() );
+  Serial.println(); 
+}

examples/vector_output/vector_output.ino

@@ -1,67 +1,75 @@
-/*
- This sketch shows to get the axis projections from the orientation estimate. It also shows how to project a vector in the global or local reference frame.
- */
-
-#include <imuFilter.h>
-#include <basicMPU6050.h>       // Library for IMU sensor. See this link: https://github.com/RCmags/basicMPU6050
-
-// Sensor fusion
-constexpr float GAIN = 0.1;     // Fusion gain, value between 0 and 1 - Determines response of heading correction with respect to gravity.
-imuFilter <&GAIN> fusion;
-
-// Imu sensor
-basicMPU6050<> imu;
-
-// Display functions:
-void printVector( vec3_t r ) {
-  Serial.print( r.x, 2 );
-  Serial.print( "," );
-  Serial.print( r.y, 2 );
-  Serial.print( "," );
-  Serial.print( r.z, 2 );
-}
-
-void printQuat( quat_t q ) {
-  Serial.print( q.w );
-  Serial.print( "," );
-  printVector( q.v );
-}
-
-void setup() {
-  // Initialize filter: 
-  fusion.setup( imu.ax(), imu.ay(), imu.az() );     
-
-  // Calibrate imu
-  imu.setup();
-  imu.setBias();
-
-  Serial.begin(38400);
-}
-
-void loop() {  
-  // Update filter:
-  fusion.update( imu.gx(), imu.gy(), imu.gz(), imu.ax(), imu.ay(), imu.az() );    
-
-  // Unit vectors of rectangular coodinates [Choose between GLOBAL_FRAME and LOCAL_FRAME]
-  vec3_t x = fusion.getXaxis(GLOBAL_FRAME);
-  vec3_t y = fusion.getYaxis(GLOBAL_FRAME);
-  vec3_t z = fusion.getZaxis(GLOBAL_FRAME);
-
-  const vec3_t VEC = {1, 1, 0};
-  vec3_t v = fusion.projectVector(VEC, GLOBAL_FRAME);
-
-  // Display vectors:
-  Serial.print( " x = " );
-  printVector( x );
-  Serial.print( " | y = " );
-  printVector( y );
-  Serial.print( " | z = " );
-  printVector( z );
-  Serial.print( " | v = " );
-  printVector( v );
-
-  // Display quaternion  
-  Serial.print( " | q = " );
-  printQuat( fusion.getQuat() );  
-  Serial.println();
-}
+/*
+ This sketch shows to get the axis projections from the orientation estimate. It also shows how to project a vector in the global or local reference frame.
+ */
+
+#include <imuFilter.h>
+#include <basicMPU6050.h>       // Library for IMU sensor. See this link: https://github.com/RCmags/basicMPU6050
+
+// Sensor fusion
+constexpr float GAIN = 0.1;     // Fusion gain, value between 0 and 1 - Determines response of heading correction with respect to gravity.
+imuFilter <&GAIN> fusion;
+
+// Imu sensor
+basicMPU6050<> imu;
+
+// Display functions:
+void printVector( float r[] ) {
+  Serial.print( r[0], 2 );
+  Serial.print( "," );
+  Serial.print( r[1], 2 );
+  Serial.print( "," );
+  Serial.print( r[2], 2 );
+}
+
+void printQuat( float q[] ) {
+  Serial.print( q[0] );
+  Serial.print( "," );
+  printVector( q + 1 );
+}
+
+void setup() {
+  // Initialize filter: 
+  fusion.setup( imu.ax(), imu.ay(), imu.az() );     
+
+  // Calibrate imu
+  imu.setup();
+  imu.setBias();
+
+  Serial.begin(38400);
+}
+
+void loop() {  
+  // Update filter:
+  fusion.update( imu.gx(), imu.gy(), imu.gz(), imu.ax(), imu.ay(), imu.az() );    
+
+  float v[3] = { 1, 1, 0 };
+  float x[3], y[3], z[3];
+
+  // Unit vectors of rectangular coodinates
+  #define TO_WORLD false         
+                                // Project local axis to global reference frame = true 
+                                // Project global axis to local reference frame = false
+
+  fusion.getXaxis( TO_WORLD, x );
+  fusion.getYaxis( TO_WORLD, y );
+  fusion.getZaxis( TO_WORLD, z );
+  fusion.projectVector( TO_WORLD, v );
+
+  // Display vectors:
+  Serial.print( " x = " );
+  printVector( x );
+  Serial.print( " | y = " );
+  printVector( y );
+  Serial.print( " | z = " );
+  printVector( z );
+  Serial.print( " | v = " );
+  printVector( v );
+
+  // Display quaternion
+  float q[4];
+  fusion.getQuat(q);
+
+  Serial.print( " | q = " );
+  printQuat( q );  
+  Serial.println();
+}

library.properties

@@ -1,10 +1,9 @@
 name=imuFilter
-version=1.1.0
+version=1.0.0
 author=RCmags <[email protected]>
 maintainer=RCmags <[email protected]>
 sentence=Sensor fusion for IMU with quaternion based filter.
 paragraph=Library that fuses the accelerometer and gyroscope outputs of an IMU. It uses a quaternion to encode the rotation and uses a modified Mahony filter to correct the gyro with the accelerometer.    
 category=Device Control
 url=https://github.com/RCmags/imuFilter
 architectures=* 
-depends=Vector datatype,