118 lines
2.7 KiB
C++
118 lines
2.7 KiB
C++
/*
|
|
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;
|
|
|
|
double gxavg;
|
|
double gyavg;
|
|
double gzavg;
|
|
|
|
//Gyro Offset....: -605622 -24028 -338386
|
|
//Gyro Offset: -6835327.00 12884521.00 918579.13
|
|
|
|
float goffx = -605622.0 / 1000000.0;
|
|
float goffy = -24028 / 1000000.0;
|
|
float goffz = -338386 / 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");
|
|
|
|
gxavg=0;
|
|
gyavg=0;
|
|
gzavg=0;
|
|
|
|
}
|
|
|
|
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);
|
|
|
|
gxavg=gx*0.0001 + gxavg*0.9999;
|
|
gyavg=gy*0.0001 + gyavg*0.9999;
|
|
gzavg=gz*0.0001 + gzavg*0.9999;
|
|
|
|
/* gx -= goffx;
|
|
gy -= goffy;
|
|
gz -= goffz;
|
|
*/
|
|
/* gx *= DEG_TO_RAD;
|
|
gy *= DEG_TO_RAD;
|
|
gz *= DEG_TO_RAD;
|
|
*/
|
|
|
|
if (random(1000)<10) {
|
|
Serial.print("Gyro Offset....: ");
|
|
Serial.print( int(gxavg*1000000.0)); Serial.print(" \t");
|
|
Serial.print( int(gyavg*1000000.0)); Serial.print(" \t");
|
|
Serial.println(int(gzavg*1000000.0));
|
|
|
|
Serial.print("Gyro Calibrated: ");
|
|
Serial.print( int((gx-goffx)*1000000.0)); Serial.print(" \t");
|
|
Serial.print( int((gy-goffy)*1000000.0)); Serial.print(" \t");
|
|
Serial.println(int((gz-goffz)*1000000.0));
|
|
}
|
|
}
|
|
}
|