109 lines
2.7 KiB
Arduino
109 lines
2.7 KiB
Arduino
|
/*
|
||
|
|
Arduino Nano 33 BLE - Sensor Fusion
|
||
|
|
|
||
|
|
This example reads values from LSM9DS1 sensor
|
||
|
|
and continuously feeds them trough the Sensor Fusion library.
|
||
|
|
|
||
|
|
The circuit:
|
||
|
|
- Arduino Nano 33 BLE Sense
|
||
|
|
|
||
|
|
This example code is in the public domain.
|
||
|
|
*/
|
||
|
|
|
||
|
|
#include <Arduino_LSM9DS1.h>
|
||
|
|
|
||
|
|
#include "SensorFusion.h" //SF
|
||
|
|
SF fusion;
|
||
|
|
|
||
|
|
float gx, gy, gz, ax, ay, az, mx, my, mz;
|
||
|
|
float pitch, roll, yaw;
|
||
|
|
float deltat;
|
||
|
|
|
||
|
|
//Gyro Offset....: -605622 -24028 -338386
|
||
|
|
//Gyro Offset: -6835327.00 12884521.00 918579.13
|
||
|
|
|
||
|
|
float goffx = -605622.0 / 1000000.0;
|
||
|
|
float goffy = -24028.0 / 1000000.0;
|
||
|
|
float goffz = -338386.0 / 1000000.0;
|
||
|
|
|
||
|
|
void setup() {
|
||
|
|
|
||
|
|
Serial.begin(115200); //serial to display data
|
||
|
|
|
||
|
|
while (!Serial);
|
||
|
|
Serial.println("Started");
|
||
|
|
|
||
|
|
// IMU begin code
|
||
|
|
|
||
|
|
if (!IMU.begin()) {
|
||
|
|
Serial.println("Failed to initialize IMU!");
|
||
|
|
while (1);
|
||
|
|
}
|
||
|
|
Serial.print("Magnetic field sample rate = ");
|
||
|
|
Serial.print(IMU.magneticFieldSampleRate());
|
||
|
|
Serial.println(" uT");
|
||
|
|
Serial.println();
|
||
|
|
Serial.println("Magnetic Field in uT");
|
||
|
|
Serial.println("X\tY\tZ");
|
||
|
|
|
||
|
|
Serial.print("Accelerometer sample rate = ");
|
||
|
|
Serial.print(IMU.accelerationSampleRate());
|
||
|
|
Serial.println(" Hz");
|
||
|
|
Serial.println();
|
||
|
|
Serial.println("Acceleration in G's");
|
||
|
|
Serial.println("X\tY\tZ");
|
||
|
|
|
||
|
|
Serial.print("Gyroscope sample rate = ");
|
||
|
|
Serial.print(IMU.gyroscopeSampleRate());
|
||
|
|
Serial.println(" Hz");
|
||
|
|
Serial.println();
|
||
|
|
Serial.println("Gyroscope in degrees/second");
|
||
|
|
Serial.println("X\tY\tZ");
|
||
|
|
|
||
|
|
|
||
|
|
|
||
|
|
}
|
||
|
|
|
||
|
|
void loop() {
|
||
|
|
// now you should read the gyroscope, accelerometer (and magnetometer if you have it also)
|
||
|
|
// NOTE: the gyroscope data have to be in radians
|
||
|
|
// if you have them in degree convert them with: DEG_TO_RAD example: gx * DEG_TO_RAD
|
||
|
|
|
||
|
|
if (IMU.magneticFieldAvailable()) {
|
||
|
|
IMU.readMagneticField(mx, my, mz);
|
||
|
|
}
|
||
|
|
|
||
|
|
if (IMU.accelerationAvailable()&&IMU.gyroscopeAvailable()) {
|
||
|
|
IMU.readAcceleration(ax, ay, az);
|
||
|
|
/*
|
||
|
|
ax *= 9.81;
|
||
|
|
ay *= 9.81;
|
||
|
|
az *= 9.81;
|
||
|
|
*/
|
||
|
|
|
||
|
|
IMU.readGyroscope(gx, gy, gz);
|
||
|
|
|
||
|
|
gx -= goffx;
|
||
|
|
gy -= goffy;
|
||
|
|
gz -= goffz;
|
||
|
|
|
||
|
|
gx *= DEG_TO_RAD;
|
||
|
|
gy *= DEG_TO_RAD;
|
||
|
|
gz *= DEG_TO_RAD;
|
||
|
|
|
||
|
|
|
||
|
|
deltat = fusion.deltatUpdate(); //this have to be done before calling the fusion update
|
||
|
|
//choose only one of these two:
|
||
|
|
//fusion.MahonyUpdate(gx, gy, gz, ax, ay, az, deltat); //mahony is suggested if there isn't the mag and the mcu is slow
|
||
|
|
fusion.MadgwickUpdate(gx, gy, gz, ax, ay, az, mx, my, mz, deltat); //else use the magwick, it is slower but more accurate
|
||
|
|
|
||
|
|
pitch = fusion.getPitch();
|
||
|
|
roll = fusion.getRoll(); //you could also use getRollRadians() ecc
|
||
|
|
yaw = fusion.getYaw();
|
||
|
|
|
||
|
|
Serial.print("Orientation: "); Serial.print(-roll);
|
||
|
|
Serial.print(" "); Serial.print(-pitch);
|
||
|
|
Serial.print(" "); Serial.println(yaw);
|
||
|
|
}
|
||
|
|
}
|