diff --git a/ArduinoNano33BLE/SensorFusionNano33BLE/SensorFusionNano33BLE.ino b/ArduinoNano33BLE/SensorFusionNano33BLE/SensorFusionNano33BLE.ino new file mode 100644 index 0000000..1310c7b --- /dev/null +++ b/ArduinoNano33BLE/SensorFusionNano33BLE/SensorFusionNano33BLE.ino @@ -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 + +#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); + } +} diff --git a/ArduinoNano33BLE/SensorFusionNano33BLE_calibration/SensorFusionNano33BLE_calibration.ino b/ArduinoNano33BLE/SensorFusionNano33BLE_calibration/SensorFusionNano33BLE_calibration.ino new file mode 100644 index 0000000..8a5cae3 --- /dev/null +++ b/ArduinoNano33BLE/SensorFusionNano33BLE_calibration/SensorFusionNano33BLE_calibration.ino @@ -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 + +#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)); + } + } +}