-
Notifications
You must be signed in to change notification settings - Fork 95
/
Nano33Madgwick.ino
69 lines (58 loc) · 1.7 KB
/
Nano33Madgwick.ino
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
/*
Madgwick orientation calculation
Uses Arduino MadgwickAHRS library to calculate heading, pitch, and roll
on an Arduino Nano 33 IoT, using the onboard LSM6DS3 IMU.
For big fun, connect this to p5.js sketch MadgwickVisualizer
created 4 Aug 2019
updated 25 Aug 2019
by Tom Igoe
*/
#include <Arduino_LSM6DS3.h>
#include <MadgwickAHRS.h>
// initialize a Madgwick filter:
Madgwick filter;
// sensor's sample rate is fixed at 104 Hz:
const float sensorRate = 104.00;
// values for orientation:
float roll = 0.0;
float pitch = 0.0;
float heading = 0.0;
void setup() {
Serial.begin(9600);
// attempt to start the IMU:
if (!IMU.begin()) {
Serial.println("Failed to initialize IMU");
// stop here if you can't access the IMU:
while (true);
}
// start the filter to run at the sample rate:
filter.begin(sensorRate);
}
void loop() {
// values for acceleration & rotation:
float xAcc, yAcc, zAcc;
float xGyro, yGyro, zGyro;
// check if the IMU is ready to read:
if (IMU.accelerationAvailable() &&
IMU.gyroscopeAvailable()) {
// read accelerometer & gyrometer:
IMU.readAcceleration(xAcc, yAcc, zAcc);
IMU.readGyroscope(xGyro, yGyro, zGyro);
// update the filter, which computes orientation:
filter.updateIMU(xGyro, yGyro, zGyro, xAcc, yAcc, zAcc);
// print the heading, pitch and roll
roll = filter.getRoll();
pitch = filter.getPitch();
heading = filter.getYaw();
}
// if you get a byte in the serial port,
// send the latest heading, pitch, and roll:
if (Serial.available()) {
char input = Serial.read();
Serial.print(heading);
Serial.print(",");
Serial.print(pitch);
Serial.print(",");
Serial.println(roll);
}
}