pifcamp-2021/ArduinoNano33BLE/SensorFusionNano33BLE/SensorFusionNano33BLE.ino

109 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;
//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);
}
}