added sensor fusion calibration and diagnostic arduino sketches for arduino nano 33 ble

eulerConversion
fileneed 2022-03-04 02:43:20 +01:00
parent bd77bfab1e
commit b2536f2b92
2 changed files with 225 additions and 0 deletions

View File

@ -0,0 +1,108 @@
/*
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);
}
}

View File

@ -0,0 +1,117 @@
/*
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));
}
}
}