Hi,
Last couple of weeks I was fiddeling arround with some code and I got it working more or less.
But to be honest, I find the results disapointing.
So I could use some help.
- Even with a noise offset (averaging) the results are very unstable.
- Also if I turn one axis, there are unrealistic peak values (fast turning) or even sometimes lower values.
- The accelerator values seem more stable and accurate then Gyro or combined, which is strange to my mind
- i'm not sure how to use some settings like filter.begin(value). it only works if the value is 10 but I don't know why.
.
Below you can see the unstability and turning 90 degrees which triggers a jump to 152 degrees and also some negative values. :o
15:54:39.470 -> 166.64, 0.12, 0.14
15:54:39.604 -> 165.84, 0.16, 0.18
15:54:39.748 -> 165.61, 0.19, -0.01
15:54:39.850 -> 165.39, 0.09, 0.28
15:54:39.990 -> 165.10, 0.11, 0.14
15:54:40.103 -> 164.96, -0.09, 0.15
15:54:40.240 -> 164.69, -0.04, 0.50
15:54:40.379 -> 166.23, 0.11, 0.44
15:54:40.491 -> 166.10, -0.18, 0.58
15:54:40.631 -> 167.93, -0.00, 0.70
15:54:40.738 -> 171.72, -0.06, 1.27
15:54:40.878 -> 172.08, 0.05, 2.90
15:54:40.983 -> 173.87, -1.19, 29.22
15:54:41.123 -> 174.39, 0.25, 67.94
15:54:41.262 -> 175.58, 4.68, -31.40
15:54:41.374 -> 175.29, 5.02, -28.89
15:54:41.518 -> 175.23, 3.16, 135.99
15:54:41.624 -> 175.25, 4.03, 143.96
15:54:41.732 -> 178.51, 8.71, 38.23
15:54:41.874 -> 174.76, 11.29, -38.32
15:54:41.986 -> 177.18, 2.32, 152.33
15:54:42.122 -> 171.24, 9.95, 29.65
15:54:42.269 -> 165.19, 0.69, 74.43
15:54:42.377 -> 164.98, 1.48, 106.26
15:54:42.490 -> 163.39, 0.54, 94.08
15:54:42.638 -> 162.76, 0.43, 94.56
15:54:42.744 -> 162.53, 0.58, 94.01
15:54:42.881 -> 162.08, 0.59, 93.44
15:54:43.015 -> 162.00, 0.56, 94.03
15:54:43.121 -> 161.57, 0.56, 93.46
15:54:43.258 -> 161.16, 0.53, 93.76
Here's my code: (i'm not a professional so be kind 
)
#include "SparkFunLSM6DS3.h"
#include "Wire.h"
//#include "SPI.h"
#include "MahonyAHRS.h"
#include "MadgwickAHRS.h"
LSM6DS3 myIMU; //Default constructor is I2C, addr 0x6B
Mahony filter; // Choose a filter
//Madgwick filter;
//int16_t accOffsetX,accOffsetY,accOffsetZ,gyroOffsetX,gyroOffsetY,gyroOffsetZ;
//int16_t accX,accY,accZ,gyroX,gyroY,gyroZ;
float accOffsetX,accOffsetY,accOffsetZ,gyroOffsetX,gyroOffsetY,gyroOffsetZ = 0;
float accX,accY,accZ,gyroX,gyroY,gyroZ;
float x, y, z, aRoll, aPitch, aHeading; //three axis acceleration data
double roll = 0.00, pitch = 0.00, heading = 0.00; //Roll & Pitch are the angles which rotate by the axis X and y
void setup() {
// put your setup code here, to run once:
Serial.begin(9600);//(9600);
delay(500); //relax...
Serial.println("Processor came out of reset.\n");
Wire.setClock(400000); // 400KHz
//Call .begin() to configure the IMU
myIMU.begin();
//myIMU.settings.gyroSampleRate = 13; //Hz. Can be: 13, 26, 52, 104, 208, 416, 833, 1666
//myIMU.settings.gyroBandWidth = 200; //Hz. Can be: 50, 100, 200, 400;
//myIMU.settings.accelSampleRate = 13; //Hz. Can be: 13, 26, 52, 104, 208, 416, 833, 1666, 3332, 6664, 13330
//myIMU.settings.accelBandWidth = 200; //Hz. Can be: 50, 100, 200, 400;
filter.begin(10);
CalcImuNoiseFloat();
//CalcImuNoiseRaw();
delay(500);
}
void loop()
{
/*
accX = myIMU.readRawAccelX() - accOffsetX;
accY = myIMU.readRawAccelY() - accOffsetY;
accZ = myIMU.readRawAccelZ() - accOffsetZ;
gyroX = myIMU.readRawGyroX() - gyroOffsetX;
gyroY = myIMU.readRawGyroY() - gyroOffsetY;
gyroZ = myIMU.readRawGyroZ() - gyroOffsetZ;
*/
accX = myIMU.readFloatAccelX() - accOffsetX;
accY = myIMU.readFloatAccelY() - accOffsetY;
accZ = myIMU.readFloatAccelZ() - accOffsetZ;
gyroX = myIMU.readFloatGyroX() - gyroOffsetX;
gyroY = myIMU.readFloatGyroY() - gyroOffsetY;
gyroZ = myIMU.readFloatGyroZ() - gyroOffsetZ;
//calculatePR();
//filter.update(accX, accY, accZ, gyroX, gyroY, gyroZ, 0, 0, 0);
filter.updateIMU(gyroX, gyroY, gyroZ, accX, accY, accZ );
//x = filter.getPitch();
//y = filter.getRoll();
//z = filter.getYaw();
/*
Serial.println("***** filter output *****");
Serial.print(" roll = ");
Serial.println(y, 2);
Serial.print(" pitch = ");
Serial.println(x, 2);
Serial.print(" Yaw = ");
Serial.println(z, 2);
Serial.println("********************");
*/
if (readyToPrint()) {
// print the heading, pitch and roll
roll = filter.getRoll();
pitch = filter.getPitch();
heading = filter.getYaw();
Serial.print(heading);
Serial.print(", ");
Serial.print(pitch);
Serial.print(", ");
Serial.println(roll);
//Serial.println("*******");
//Serial.print(" aRoll = ");
//Serial.println(aRoll, 2);
//Serial.print(" aPitch = ");
//Serial.println(aPitch, 2);
}
}
void CalcImuNoiseFloat(){
Serial.print(" **** Function: CalcImuNoiseFloat()");
float xa, ya, za, xg, yg, zg;
int iterations = 200;
xa = 0;
ya = 0;
za = 0;
// ----------- Acc offset
for (int i = 0; i < iterations; i++) {
xa = xa + myIMU.readFloatAccelX();
ya = ya + myIMU.readFloatAccelY();
za = za + myIMU.readFloatAccelZ();
}
// ----------- Gyro offset
for (int i = 0; i < iterations; i++) {
xg = xg + myIMU.readFloatGyroX();
yg = yg + myIMU.readFloatGyroY();
zg = zg + myIMU.readFloatGyroZ();
}
accOffsetX = xa/iterations;
accOffsetY = ya/iterations;
accOffsetZ = za/iterations - 1;
gyroOffsetX = xg/iterations;
gyroOffsetY = yg/iterations;
gyroOffsetZ = zg/iterations;
Serial.print(" accOffsetX: ");
Serial.print(accOffsetX,4);
Serial.print(" accOffsetY: ");
Serial.print(accOffsetY,4);
Serial.print(" accOffsetZ: ");
Serial.print(accOffsetZ,4);
Serial.println();
// ------------------
Serial.print("*** Gyro offSet ***");
Serial.print(" gyroOffsetX: ");
Serial.print(gyroOffsetX,4);
Serial.print(" gyroOffsetY: ");
Serial.print(gyroOffsetY,4);
Serial.print(" gyroOffsetZ: ");
Serial.print(gyroOffsetZ,4);
Serial.println();
}
void CalcImuNoiseRaw(){
Serial.print(" **** Function: CalcImuNoiseRaw()");
int16_t xa, ya, za, xg, yg, zg;
int iterations = 200;
xa = 0;
ya = 0;
za = 0;
// ----------- Acc offset
for (int i = 0; i < iterations; i++) {
xa = xa + myIMU.readRawAccelX();
ya = ya + myIMU.readRawAccelY();
za = za + myIMU.readRawAccelZ();
}
// ----------- Gyro offset
for (int i = 0; i < iterations; i++) {
xg = xg + myIMU.readRawGyroX();
yg = yg + myIMU.readRawGyroY();
zg = zg + myIMU.readRawGyroZ();
}
accOffsetX = xa/iterations;
accOffsetY = ya/iterations;
accOffsetZ = za/iterations; // - 1;
gyroOffsetX = xg/iterations;
gyroOffsetY = yg/iterations;
gyroOffsetZ = zg/iterations;
Serial.print(" accOffsetX: ");
Serial.print(accOffsetX,4);
Serial.print(" accOffsetY: ");
Serial.print(accOffsetY,4);
Serial.print(" accOffsetZ: ");
Serial.print(accOffsetZ,4);
Serial.println();
// ------------------
Serial.print("*** Gyro offSet ***");
Serial.print(" gyroOffsetX: ");
Serial.print(gyroOffsetX,4);
Serial.print(" gyroOffsetY: ");
Serial.print(gyroOffsetY,4);
Serial.print(" gyroOffsetZ: ");
Serial.print(gyroOffsetZ,4);
Serial.println();
}
void calculatePR(){
double x_Buff = float(accX);
double y_Buff = float(accY);
double z_Buff = float(accZ);
aRoll = atan2(y_Buff , z_Buff) * 57.295779513082321;
aPitch = atan2((- x_Buff) , sqrt(y_Buff * y_Buff + z_Buff * z_Buff)) * 57.295779513082321;
}
// Decide when to print
bool readyToPrint() {
static unsigned long nowMillis;
static unsigned long thenMillis;
// If the Processing visualization sketch is sending "s"
// then send new data each time it wants to redraw
while (Serial.available()) {
int val = Serial.read();
if (val == 's') {
thenMillis = millis();
return true;
}
}
// Otherwise, print 8 times per second, for viewing as
// scrolling numbers in the Arduino Serial Monitor
nowMillis = millis();
if (nowMillis - thenMillis > 125) {
thenMillis = nowMillis;
return true;
}
return false;
}
plus shipping... ok granted it has a 6 to 8 week shipping time from china, but guess where the other chips are coming from 