Gyroscope + Complementary filter (inaccurate, starts printing nan)

Using Arduino Programming Questions
#1

Hey guys! First time poster, and a semi-noob here. I'm currently working on a posture trainer, where I aim to have two gyroscopes placed on the user's spine, comparing their values to determine whether or not he/she is slouching.

So, I've been trawling around forums and guides in order to get pitch data along the y axis of two gyroscopes working. Finally, I've been able to get it to work somewhat, but it's got issues.

First and foremost, after running for some time (10-30 seconds), the console starts printing not a number(nan).
Second, it's not very accurate. Small, slow changes don't register at all. I need it to be accurate since just a degree or two can screw it up for my application of choice.

I am using SparkFun LSM6DS3 6DOF boards (Datasheet (PDF download)).
This is my test code (cred to racquemis here on the forums for the complementary filter code).
Since I am using two gyros, this code also implements SparkFun's example for hooking up multiple units over I2C. I am only measuring one for this test.

#include "SparkFunLSM6DS3.h"
#include "Wire.h"
#include "SPI.h"

//Create two instances of the driver class
LSM6DS3 SensorOne( I2C_MODE, 0x6A );
LSM6DS3 SensorTwo( I2C_MODE, 0x6B );

long timer = micros();
int delta_t;

float pitch = 0;
float pitchAcc;
float P_CompCoeff= 0.98;

float gyro1Rotation;
float gyro2Rotation;

void setup() {
  // put your setup code here, to run once:
  Serial.begin(9600);
  delay(5000); //relax...
  Serial.println("Processor came out of reset.\n");
  
  //Call .begin() to configure the IMUs
  if( SensorOne.begin() != 0 )
  {
	  Serial.println("Problem starting the sensor at 0x6A.");
  }
  else
  {
	  Serial.println("Sensor at 0x6A started.");
  }
  if( SensorTwo.begin() != 0 )
  {
	  Serial.println("Problem starting the sensor at 0x6B.");
  }
  else
  {
	  Serial.println("Sensor at 0x6B started.");
  }
  
}


void loop()
{
  ComplementaryFilter(SensorTwo.readFloatAccelX(), SensorTwo.readFloatAccelY(), SensorTwo.readFloatAccelZ(), SensorTwo.readFloatGyroY());
  timer=micros();
  Serial.println(pitch);
  delay(20);
}

void ComplementaryFilter(float ax,float ay,float az,float gy) {
 delta_t = micros() - timer;
 long squaresum=(long)ay*ay+(long)az*az;
 pitch+=((-gy/32.8f)*(delta_t/1000000.0f)); 
 pitchAcc =atan(ax/sqrt(squaresum))*RAD_TO_DEG;
 pitch =P_CompCoeff*pitch + (1.0f-P_CompCoeff)*pitchAcc;
}

The gyroscopes he uses are different, so I guess that the inaccuracy could have something to do with his gyro having a different sensitivity?

Would be eternally grateful for any help! If I've missed providing you with some piece of information, feel free to gimme a holler and I'll do my best to fill you in!

Cheers!

#2

when one is using basic trig functions, NaN usually comes from a run-time divide by zero.

#3

Get the code working correctly with just one sensor, before attempting to implement two. It is absolutely essential that you understand the differences in sensitivities and make any changes necessary.

Let us know if you have difficulty with the simplified project.

#4

jremington:
Get the code working correctly with just one sensor, before attempting to implement two. It is absolutely essential that you understand the differences in sensitivities and make any changes necessary.

Let us know if you have difficulty with the simplified project.

As I said above, only one gyro is active at the moment. The code only initializes one of them, since only one is hooked up.
I'm now running the code with everything relating to the second gyro commented out.

wg0z:
when one is using basic trig functions, NaN usually comes from a run-time divide by zero.

Hmm... I guess this could possibly be a result of the accelerometer outputting 0 in one axis. Been running the gyro at an angle for a couple of minutes now without it reporting nan, so I guess that was the issue! Gonna write a snippet which discards the result if it returns as NaN.

Still leaves the inaccuracy though. No clue how to fix that. In the forum post where I found this code, the poster says that he used the value 32.8f in the equation because of his gyroscope's 1000 degrees/second sensitivity. The data sheet for mine lists multiple sensitivities, with 1000dps being one of them. Could this be the issue?

#5

I feel some clarification is needed. After modifying the code a bit and testing thoroughly, I'd rather say that it's very accurate, and doesn't fluctuate much at all when held still. The issue is that a less-than 30 degree rotation of the gyro results in a 180 degree rotation on screen.

If I hold it down flat on my table, it will hover around 0 (+/-2). If I tilt it very slightly, however, it will spike up extremely quickly at about +/-10 degrees of reported rotation. I've tested this using both gyros, and it makes no difference, which rules out hardware malfuncion.

Any ideas?

#6

It is absolutely essential that you understand the differences in sensitivities and make any changes necessary.

#7

jremington:

Sorry for the late reply. I've managed to come down with pneumonia at the worst possible time.

I'm asking for help in understanding these sensitivities, and how to make appropriate changes. I've read through as much as I could find, and found some documentation indicating that the gyro operates at 2000dps, and have changed the sensitivity setting in the code accordingly. This changed very little.

At the moment, the gyro seems to work between -10 to +10 degrees, but then spikes up to 90/-90 degrees pretty much instantly.

#8