Skip to content

Commit 9cd86c4

Browse files
RCmagsRCmags
committed
downgrade to v1.0.0
1 parent 772aa35 commit 9cd86c4

File tree

8 files changed

+592
-419
lines changed

8 files changed

+592
-419
lines changed

‎README.md‎

Lines changed: 0 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -21,7 +21,3 @@ Since a 6-axis IMU has no absolute reference for heading there is a function to
2121
For more information on the Mahony filter see these links:
2222
- [IMU Data Fusing: Complementary, Kalman, and Mahony Filter](http://www.olliw.eu/2013/imu-data-fusing/#chapter23)
2323
- [Mahony Filter](https://nitinjsanket.github.io/tutorials/attitudeest/mahony)
24-
25-
## Dependencies
26-
27-
This library depends on the [vector_datatype](https://github.com/RCmags/vector_datatype) library.

‎examples/heading/heading.ino‎

Lines changed: 80 additions & 41 deletions
Original file line numberDiff line numberDiff line change
@@ -1,41 +1,80 @@
1-
/*
2-
This sketch shows to perform vector products and rotate heading (yaw angle) of the estimated orientation.
3-
*/
4-
5-
#include <imuFilter.h>
6-
#include <basicMPU6050.h> // Library for IMU sensor. See this link: https://github.com/RCmags/basicMPU6050
7-
8-
// Sensor fusion
9-
constexpr float GAIN = 0.1; // Fusion gain, value between 0 and 1 - Determines response of heading correction with respect to gravity.
10-
imuFilter <&GAIN> fusion;
11-
12-
// Imu sensor
13-
basicMPU6050<> imu;
14-
15-
void setup() {
16-
// Initialize filter:
17-
fusion.setup( imu.ax(), imu.ay(), imu.az() );
18-
19-
// Calibrate imu
20-
imu.setup();
21-
imu.setBias();
22-
23-
Serial.begin(38400);
24-
25-
// Rotate heading:
26-
float angle = 45 * DEG_TO_RAD; // angle in radians to rotate heading about z-axis
27-
fusion.rotateHeading( angle, LARGE_ANGLE ); // Can choose LARGE_ANGLE or SMALL_ANGLE approximation
28-
}
29-
30-
void loop() {
31-
// Update filter:
32-
fusion.update( imu.gx(), imu.gy(), imu.gz(), imu.ax(), imu.ay(), imu.az() );
33-
34-
// Display angles:
35-
Serial.print( fusion.pitch() );
36-
Serial.print( " " );
37-
Serial.print( fusion.yaw() );
38-
Serial.print( " " );
39-
Serial.print( fusion.roll() );
40-
Serial.println();
41-
}
1+
/*
2+
This sketch shows to perform vector products and rotate heading (yaw angle) of the estimated orientation.
3+
*/
4+
5+
#include <imuFilter.h>
6+
#include <basicMPU6050.h> // Library for IMU sensor. See this link: https://github.com/RCmags/basicMPU6050
7+
8+
// Sensor fusion
9+
constexpr float GAIN = 0.1; // Fusion gain, value between 0 and 1 - Determines response of heading correction with respect to gravity.
10+
imuFilter <&GAIN> fusion;
11+
12+
// Imu sensor
13+
basicMPU6050<> imu;
14+
15+
// Display function:
16+
void printVector( float r[] ) {
17+
Serial.print( r[0], 2 );
18+
Serial.print( "," );
19+
Serial.print( r[1], 2 );
20+
Serial.print( "," );
21+
Serial.print( r[2], 2 );
22+
Serial.println();
23+
}
24+
25+
void setup() {
26+
// Initialize filter:
27+
fusion.setup( imu.ax(), imu.ay(), imu.az() );
28+
29+
// Calibrate imu
30+
imu.setup();
31+
imu.setBias();
32+
33+
Serial.begin(38400);
34+
35+
// Vector operations:
36+
float v1[] = { 3, 1, -1 }; // Input Vector
37+
float axis_y[3], axis_z[3];
38+
39+
fusion.getYaxis( true, axis_y ); // Vectors to operate on [global axes]
40+
fusion.getZaxis( true, axis_z );
41+
42+
fusion.crossProduct( v1, axis_y ); // Cross product: V = V cross R ; Output is stored in V
43+
float v2[] = { v1[0], v1[1], v1[2] }; // Store product
44+
45+
fusion.normalizeVector( v1 ); // Norm: V = V/|V| ; Output is stored in V
46+
47+
float dot = fusion.dotProduct( v1, axis_z ); // Dot product: Input order does not matter
48+
49+
// Rotate heading:
50+
#define SMALL_ANG false
51+
// Small angle approximation = true
52+
// Exact angle rotation = false
53+
fusion.rotateHeading( SMALL_ANG, dot );
54+
55+
// Display results:
56+
Serial.print( "y = " );
57+
printVector( axis_y );
58+
Serial.print( "v2 = " );
59+
printVector( v2 );
60+
Serial.print( "v1 = " );
61+
printVector( v1 );
62+
Serial.print( "dot = ");
63+
Serial.println( dot );
64+
65+
// Wait for output to be read
66+
delay(10000);
67+
}
68+
69+
void loop() {
70+
// Update filter:
71+
fusion.update( imu.gx(), imu.gy(), imu.gz(), imu.ax(), imu.ay(), imu.az() );
72+
73+
// Display angles:
74+
Serial.print( fusion.pitch() );
75+
Serial.print( " " );
76+
Serial.print( fusion.yaw() );
77+
Serial.print( " " );
78+
Serial.print( fusion.roll() );
79+
Serial.println();
80+
}

‎examples/output/output.ino‎

Lines changed: 52 additions & 52 deletions
Original file line numberDiff line numberDiff line change
@@ -1,52 +1,52 @@
1-
/*
2-
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.
3-
*/
4-
5-
#include <imuFilter.h>
6-
#include <basicMPU6050.h> // Library for IMU sensor. See this link: https://github.com/RCmags/basicMPU6050
7-
8-
// Sensor fusion
9-
constexpr float GAIN = 0.1; // Fusion gain, value between 0 and 1 - Determines orientation correction with respect to gravity vector.
10-
imuFilter <&GAIN> fusion; // If set to 1 the gyroscope is dissabled. If set to 0 the accelerometer is dissabled (equivant to gyro-only).
11-
12-
// Imu sensor
13-
basicMPU6050<> imu;
14-
15-
// Enable sensor fusion. Setting to "true" enables gravity correction.
16-
#define FUSION false
17-
18-
void setup() {
19-
#if FUSION
20-
// Set quaternion with gravity vector
21-
fusion.setup( imu.ax(), imu.ay(), imu.az() );
22-
#else
23-
// Just use gyro
24-
fusion.setup();
25-
#endif
26-
27-
// Calibrate imu
28-
imu.setup();
29-
imu.setBias();
30-
31-
Serial.begin(38400);
32-
}
33-
34-
void loop() {
35-
// Update filter:
36-
37-
#if FUSION
38-
//Fuse gyro and accelerometer
39-
fusion.update( imu.gx(), imu.gy(), imu.gz(), imu.ax(), imu.ay(), imu.az() );
40-
#else
41-
// Only use gyroscope
42-
fusion.update( imu.gx(), imu.gy(), imu.gz() );
43-
#endif
44-
45-
// Display angles:
46-
Serial.print( fusion.pitch() );
47-
Serial.print( " " );
48-
Serial.print( fusion.yaw() );
49-
Serial.print( " " );
50-
Serial.print( fusion.roll() );
51-
Serial.println();
52-
}
1+
/*
2+
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.
3+
*/
4+
5+
#include <imuFilter.h>
6+
#include <basicMPU6050.h> // Library for IMU sensor. See this link: https://github.com/RCmags/basicMPU6050
7+
8+
// Sensor fusion
9+
constexpr float GAIN = 0.1; // Fusion gain, value between 0 and 1 - Determines heading correction with respect to gravity vector.
10+
imuFilter <&GAIN> fusion;
11+
12+
// Imu sensor
13+
basicMPU6050<> imu;
14+
15+
// Enable sensor fusion [ 1 = yes; 0 = no]
16+
#define FUSION 1
17+
18+
void setup() {
19+
#if FUSION
20+
// Set quaternion with gravity vector
21+
fusion.setup( imu.ax(), imu.ay(), imu.az() );
22+
#else
23+
// Just use gyro
24+
fusion.setup();
25+
#endif
26+
27+
// Calibrate imu
28+
imu.setup();
29+
imu.setBias();
30+
31+
Serial.begin(38400);
32+
}
33+
34+
void loop() {
35+
// Update filter:
36+
37+
#if FUSION
38+
//Fuse gyro and accelerometer
39+
fusion.update( imu.gx(), imu.gy(), imu.gz(), imu.ax(), imu.ay(), imu.az() );
40+
#else
41+
// Only use gyroscope
42+
fusion.update( imu.gx(), imu.gy(), imu.gz() );
43+
#endif
44+
45+
// Display angles:
46+
Serial.print( fusion.pitch() );
47+
Serial.print( " " );
48+
Serial.print( fusion.yaw() );
49+
Serial.print( " " );
50+
Serial.print( fusion.roll() );
51+
Serial.println();
52+
}

‎examples/vector_output/vector_output.ino‎

Lines changed: 75 additions & 67 deletions
Original file line numberDiff line numberDiff line change
@@ -1,67 +1,75 @@
1-
/*
2-
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.
3-
*/
4-
5-
#include <imuFilter.h>
6-
#include <basicMPU6050.h> // Library for IMU sensor. See this link: https://github.com/RCmags/basicMPU6050
7-
8-
// Sensor fusion
9-
constexpr float GAIN = 0.1; // Fusion gain, value between 0 and 1 - Determines response of heading correction with respect to gravity.
10-
imuFilter <&GAIN> fusion;
11-
12-
// Imu sensor
13-
basicMPU6050<> imu;
14-
15-
// Display functions:
16-
void printVector( vec3_t r ) {
17-
Serial.print( r.x, 2 );
18-
Serial.print( "," );
19-
Serial.print( r.y, 2 );
20-
Serial.print( "," );
21-
Serial.print( r.z, 2 );
22-
}
23-
24-
void printQuat( quat_t q ) {
25-
Serial.print( q.w );
26-
Serial.print( "," );
27-
printVector( q.v );
28-
}
29-
30-
void setup() {
31-
// Initialize filter:
32-
fusion.setup( imu.ax(), imu.ay(), imu.az() );
33-
34-
// Calibrate imu
35-
imu.setup();
36-
imu.setBias();
37-
38-
Serial.begin(38400);
39-
}
40-
41-
void loop() {
42-
// Update filter:
43-
fusion.update( imu.gx(), imu.gy(), imu.gz(), imu.ax(), imu.ay(), imu.az() );
44-
45-
// Unit vectors of rectangular coodinates [Choose between GLOBAL_FRAME and LOCAL_FRAME]
46-
vec3_t x = fusion.getXaxis(GLOBAL_FRAME);
47-
vec3_t y = fusion.getYaxis(GLOBAL_FRAME);
48-
vec3_t z = fusion.getZaxis(GLOBAL_FRAME);
49-
50-
const vec3_t VEC = {1, 1, 0};
51-
vec3_t v = fusion.projectVector(VEC, GLOBAL_FRAME);
52-
53-
// Display vectors:
54-
Serial.print( " x = " );
55-
printVector( x );
56-
Serial.print( " | y = " );
57-
printVector( y );
58-
Serial.print( " | z = " );
59-
printVector( z );
60-
Serial.print( " | v = " );
61-
printVector( v );
62-
63-
// Display quaternion
64-
Serial.print( " | q = " );
65-
printQuat( fusion.getQuat() );
66-
Serial.println();
67-
}
1+
/*
2+
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.
3+
*/
4+
5+
#include <imuFilter.h>
6+
#include <basicMPU6050.h> // Library for IMU sensor. See this link: https://github.com/RCmags/basicMPU6050
7+
8+
// Sensor fusion
9+
constexpr float GAIN = 0.1; // Fusion gain, value between 0 and 1 - Determines response of heading correction with respect to gravity.
10+
imuFilter <&GAIN> fusion;
11+
12+
// Imu sensor
13+
basicMPU6050<> imu;
14+
15+
// Display functions:
16+
void printVector( float r[] ) {
17+
Serial.print( r[0], 2 );
18+
Serial.print( "," );
19+
Serial.print( r[1], 2 );
20+
Serial.print( "," );
21+
Serial.print( r[2], 2 );
22+
}
23+
24+
void printQuat( float q[] ) {
25+
Serial.print( q[0] );
26+
Serial.print( "," );
27+
printVector( q + 1 );
28+
}
29+
30+
void setup() {
31+
// Initialize filter:
32+
fusion.setup( imu.ax(), imu.ay(), imu.az() );
33+
34+
// Calibrate imu
35+
imu.setup();
36+
imu.setBias();
37+
38+
Serial.begin(38400);
39+
}
40+
41+
void loop() {
42+
// Update filter:
43+
fusion.update( imu.gx(), imu.gy(), imu.gz(), imu.ax(), imu.ay(), imu.az() );
44+
45+
float v[3] = { 1, 1, 0 };
46+
float x[3], y[3], z[3];
47+
48+
// Unit vectors of rectangular coodinates
49+
#define TO_WORLD false
50+
// Project local axis to global reference frame = true
51+
// Project global axis to local reference frame = false
52+
53+
fusion.getXaxis( TO_WORLD, x );
54+
fusion.getYaxis( TO_WORLD, y );
55+
fusion.getZaxis( TO_WORLD, z );
56+
fusion.projectVector( TO_WORLD, v );
57+
58+
// Display vectors:
59+
Serial.print( " x = " );
60+
printVector( x );
61+
Serial.print( " | y = " );
62+
printVector( y );
63+
Serial.print( " | z = " );
64+
printVector( z );
65+
Serial.print( " | v = " );
66+
printVector( v );
67+
68+
// Display quaternion
69+
float q[4];
70+
fusion.getQuat(q);
71+
72+
Serial.print( " | q = " );
73+
printQuat( q );
74+
Serial.println();
75+
}

‎library.properties‎

Lines changed: 1 addition & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -1,10 +1,9 @@
11
name=imuFilter
2-
version=1.1.0
2+
version=1.0.0
33
author=RCmags <[email protected]>
44
maintainer=RCmags <[email protected]>
55
sentence=Sensor fusion for IMU with quaternion based filter.
66
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.
77
category=Device Control
88
url=https://github.com/RCmags/imuFilter
99
architectures=*
10-
depends=Vector datatype,

0 commit comments

Comments
 (0)