wrong asin() values during tilt detection with MMA7361L accelerometer

Hi everybody! This is my forst post in the forum, even if I’ve been reading it for a while :grin:

I wrote this bunch of code to detect a g-variation and tilt of a Freescale MMA7361L accelerometer. I also implemented some sort of autozero technique to calibrate the accelerometer befor doing the measurement.

#include <math.h>
#define M_PI 3.1415926535897932
/*
microbot.it - Freescale MMA7361L
autozero
g measure
tilt measure
*/


int xPin = 0;
int yPin = 1;
int zPin = 2;
int selPin = 7;
int sleepPin = 6;

float xValue, yValue, zValue;
float gxValue , gyValue, gzValue;
float degxValue, degyValue, degzValue;;
const float scalePoints = 1024/5*3.3;

float calValues[3] = {0,0,0};
int numReadings = 0;
int maxNumReadings = 20; // auto-zero number of readings
long dt = 250;
long lastTime = 0;


void setup() {
  Serial.begin(9700);
  digitalWrite(sleepPin,HIGH);
  digitalWrite(selPin,LOW); 
  getCalValues(); // autozero calibration
}

void loop() {
  
  xValue = analogRead(xPin);
  yValue = analogRead(yPin);
  zValue = analogRead(zPin);
  
  gxValue = (xValue/scalePoints*3.3-1.65-calValues[0])/0.8; // calculate g values
  gyValue = (yValue/scalePoints*3.3-1.65-calValues[1])/0.8;
  gzValue = (zValue/scalePoints*3.3-1.65-calValues[2])/0.8;
  
  degxValue = asin(gxValue)/M_PI*180; // calculate tilt
  degyValue = asin(gyValue)/M_PI*180;
  degzValue = asin(gzValue)/M_PI*180;
  
  Serial.print(gxValue); Serial.print("   ");  // print g values
  Serial.print(gyValue); Serial.print("   ");
  Serial.print(gzValue);
  Serial.println("\t");
  
  Serial.print(degxValue); Serial.print("   "); // print tilt
  Serial.print(degyValue); Serial.print("   ");
  Serial.print(degzValue);
  Serial.println("\t"); Serial.println("\t");  
  
  delay(2000);
}


void getCalValues() {
  while (numReadings < maxNumReadings)
    if (millis() - lastTime > dt) {
      calValues[0] = calValues[0] + analogRead(xPin);
      calValues[1] = calValues[1] + analogRead(yPin);
      calValues[2] = calValues[2] + analogRead(zPin);
      lastTime = millis();
      numReadings++;
  }
  calValues[0] = (calValues[0] / maxNumReadings)/scalePoints*3.3-1.65;
  calValues[1] = (calValues[1] / maxNumReadings)/scalePoints*3.3-1.65;
  calValues[2] = (calValues[2] / maxNumReadings)/scalePoints*3.3-0.85;
}

At the serial output I print two lines

  • the first line contains the g values of the three axes
  • the second one the tilt, where I take the asin() of the g values

I’ve this problem: when I take asin(x) the asin returns some wrong values, look at the third column:

0.00   0.01   -1.00	
0.21   0.61   0.00	
	
0.00   -0.00   -1.00	
0.21   -0.09   -85.74	
	
0.02   0.01   -1.00	
0.91   0.61   0.00	
	
0.00   0.02   -1.00	
0.21   0.97   -85.74	
	
-0.00   0.00   -1.00	
-0.14   0.26   -85.74	
	
-0.00   -0.01   -1.01	
-0.14   -0.44   0.00	
	
-0.00   -0.01   -0.99	
-0.14   -0.44   -82.36	
	
-0.01   0.01   -1.00	
-0.49   0.61   -85.74	
	
-0.01   -0.00   -1.01	
-0.49   -0.09   0.00	
	
-0.00   -0.01   -1.00	
-0.14   -0.79   0.00	
	
-0.00   0.00   -1.00	
-0.14   0.26   0.00	
	
0.01   -0.00   -1.00	
0.56   -0.09   -85.74	
	
-0.01   0.01   -1.00	
-0.84   0.61   0.00	
	
0.02   0.00   -1.00	
0.91   0.26   0.00

asin(-1) is not 0, asin(-1) is not -85.74 ect … what am I doing wrong? can someone help me out? maybe I’m using wrong variable types?

Thanks :astonished:

sounds like a precision issue

const float scalePoints = 1024/5*3.3; if evaluated from left to right is possibly not what you intended ==>

const float scalePoints = 3.3 * (1024.0/5.0);

and probably you need to change 1024 in 1023 as that is the max value of the ADC not 1024 (although it's effect is ~ 0.1% )

further - Serial.begin(9700); most people do 9600 or 115200 but OK

  • the factor 3.3 is both in the const scalePoints and in the formula is that correct? gyValue = (yValue/scalePoints * 3.3 - 1.65 - calValues[1] )/0.8;

  • these formulas differ 1.65 and 0.85, while on the other places everywhere 1.65 is used (broken "pattern" ) calValues[1] = (calValues[1] / maxNumReadings)/scalePoints*3.3-1.65; calValues[2] = (calValues[2] / maxNumReadings)/scalePoints*3.3-0.85;

just some observations

@robtillaart
thanks for the observations, I changed 1024 to 1023 and the baudrate as you suggested but unfortunately it didnt solve the problem

  • 1.65 and 0.85 are different and it’s right because of the placement of the accelerometer with respect of the three xyz axes

-const float scalePoints = 1024/5*3.3;
is right (I know that this expression is 1024/5 and then * 3.3 and not 1024 / (5*3.3) but thanks anyway)

  • I also changed the code of scalePoints to make it more clear (I just reused one I previously wrote, lazy…) but both the 3.3 factor were canceling out and that was as intended. Here is the new code:
#include <math.h>
#define M_PI 3.1415926535897932
/*
microbot.it - Freescale MMA7361L
autozero
g measure
tilt measure
*/


int xPin = 0;
int yPin = 1;
int zPin = 2; 
int selPin = 7;
int sleepPin = 6;

float xValue, yValue, zValue;
float gxValue , gyValue, gzValue;
float degxValue, degyValue, degzValue;;
const float scalePoints = 1023/5;

float calValues[3] = {0,0,0};
int numReadings = 0;
int maxNumReadings = 20;
long dt = 250;
long lastTime = 0;


void setup() {
  Serial.begin(9600);
  digitalWrite(sleepPin,HIGH);
  digitalWrite(selPin,LOW); 
  getCalValues();
}

void loop() {
  
  xValue = analogRead(xPin);
  yValue = analogRead(yPin);
  zValue = analogRead(zPin);
  
  gxValue = (xValue/scalePoints-1.65-calValues[0])/0.8;
  gyValue = (yValue/scalePoints-1.65-calValues[1])/0.8;
  gzValue = (zValue/scalePoints-1.65-calValues[2])/0.8;
  
  degxValue = asin(gxValue)/M_PI*180;
  degyValue = asin(gyValue)/M_PI*180;
  degzValue = asin(gzValue)/M_PI*180;
  
  Serial.print(gxValue); Serial.print("   ");
  Serial.print(gyValue); Serial.print("   ");
  Serial.print(gzValue);
  Serial.println("\t");
  
  Serial.print(degxValue); Serial.print("   ");
  Serial.print(degyValue); Serial.print("   ");
  Serial.print(degzValue);
  Serial.println("\t"); Serial.println("\t");  
  
  Serial.println(asin(-1)/M_PI*180); //test
  
  delay(2000);
}


void getCalValues() {
  while (numReadings < maxNumReadings)
    if (millis() - lastTime > dt) {
      calValues[0] = calValues[0] + analogRead(xPin);
      calValues[1] = calValues[1] + analogRead(yPin);
      calValues[2] = calValues[2] + analogRead(zPin);
      lastTime = millis();
      numReadings++;
  }
  calValues[0] = (calValues[0] / maxNumReadings)/scalePoints-1.65;
  calValues[1] = (calValues[1] / maxNumReadings)/scalePoints-1.65;
  calValues[2] = (calValues[2] / maxNumReadings)/scalePoints-0.85;
}

I added a Serial.println(asin(-1)/M_PI*180); //test line and it returns the correct value.

I looked at the link you posted about the asin and other trigonometric functions. Maybe even if in

-0.00   0.00   -1.00	
-0.14   0.26   -85.74

-1.00 looks to be -1, in the real number world is greater than -1 (e.g. -1.0000001) and the asin() function returns a wrong value? do you think is possible? Serial.println(asin(-1.00000001)/M_PI*180) returns 0.00 instead of -1 so maybe this is the problem. What do you think? Should I try to limit the input domain?

What angles are you trying to calculate? Surely there should be a dot- or cross-product somewhere in there?

MarkT: What angles are you trying to calculate? Surely there should be a dot- or cross-product somewhere in there?

@MarkT I calculate the xyz tilt angles. The corresponding g-values are correct so problem is the asin() function I guess. What "dot- or cross-product" are you talking about?

-1.00 looks to be -1, in the real number world is greater than -1 (e.g. -1.0000001) and the asin() function returns a wrong value?

That is easy to test

void setup()
{
Serial.begin(9600);
Serial.println(asin(-1.1), 8);  // prints with 8 digits iso default 2
Serial.println(asin(-1.01), 8);  
Serial.println(asin(-1.001), 8);  
Serial.println(asin(-1.0001), 8);  
Serial.println(asin(-1.00001), 8);  
Serial.println(asin(-1.000001), 8);  
Serial.println(asin(-1.0000001), 8);  
}

void loop(){};

Define exactly what you mean by "xyz tilt angles" - it seems you are applying asin where acos would be the obvious choice?

3D geometry is usually all dot- and cross-products BTW

@robtillaart I did that test, it is definitely because the input domain of asin() function is not constrained between -1,1. This is probably due to the calibration technique that introduces an offset that takes the values close to 1 slightly greater than one. I'll fix that and post the final version.

@MarkT Sorry MarkT, now I understand what you ment. You can find a very clear guide to tilt detection in this white paper AN3107 that you can find at http://www.freescale.com/webapp/sps/site/prod_summary.jsp?code=MMA7361LC&tab=Documentation_Tab&pspll=1&SelectedAsset=Documentation&ProdMetaId=PID/DC/MMA7361LC&fromPSP=true&assetLockedForNavigation=true&componentId=2&leftNavCode=1&pageSize=25&Documentation=Documentation/00210KscRcb%60%60Application%20Notes&fpsp=1&linkline=Application%20Notes

This is the final correct code. Basically, what it does is the auto-zero calibration technique, it receives the g-values from the accelerometer and it calculates the tilt degrees for the xyz axes as specified on the MMA7361L chip. Wiring is straithforward.

#include <math.h>
#define M_PI 3.1415926535897932
/*
microbot.it - Freescale MMA7361L
autozero
g measure
tilt measure
*/


int xPin = 0;
int yPin = 1;
int zPin = 2; 
int selPin = 7;
int sleepPin = 6;

float xValue, yValue, zValue;
float gxValue, gyValue, gzValue;
float degxValue, degyValue, degzValue;;
const float scalePoints = 1023/5;

float calValues[3] = {0,0,0};
int numReadings = 0;
int maxNumReadings = 20;
long dt = 250;
long lastTime = 0;


void setup() {
  Serial.begin(9600);
  digitalWrite(sleepPin,HIGH);
  digitalWrite(selPin,LOW); 
  getCalValues();
}

void loop() {
  
  xValue = analogRead(xPin);
  yValue = analogRead(yPin);
  zValue = analogRead(zPin);
  
  gxValue = constrain((xValue/scalePoints-1.65-calValues[0])/0.8,-1,1);
  gyValue = constrain((yValue/scalePoints-1.65-calValues[1])/0.8,-1,1);
  gzValue = constrain((zValue/scalePoints-1.65-calValues[2])/0.8,-1,1);
  
  degxValue = asin(gxValue)/M_PI*180;
  degyValue = asin(gyValue)/M_PI*180;
  degzValue = asin(gzValue)/M_PI*180;
  
  Serial.print(gxValue,10); Serial.print("   ");
  Serial.print(gyValue,10); Serial.print("   ");
  Serial.print(gzValue,10);
  Serial.println("\t");
  
  Serial.print(degxValue,10); Serial.print("   ");
  Serial.print(degyValue,10); Serial.print("   ");
  Serial.print(degzValue,10);
  Serial.println("\t"); Serial.println("\t");  
 
  delay(2000);
}


void getCalValues() {
  while (numReadings < maxNumReadings)
    if (millis() - lastTime > dt) {
      calValues[0] = calValues[0] + analogRead(xPin);
      calValues[1] = calValues[1] + analogRead(yPin);
      calValues[2] = calValues[2] + analogRead(zPin);
      lastTime = millis();
      numReadings++;
  }
  calValues[0] = (calValues[0] / maxNumReadings)/scalePoints-1.65;
  calValues[1] = (calValues[1] / maxNumReadings)/scalePoints-1.65;
  calValues[2] = (calValues[2] / maxNumReadings)/scalePoints-0.85;
}

I'm still confused by this code - why aren't you converting the incoming x,y,z to a unit vector first? Scaling to a unit vector preserves angle, truncating to the range -1..+1 doesn't.

Also why the calibration has a 0.85V value just for Z? You just get way-off values for Z, surely?

@MarkT

Sorry for being short, I’ll explain the whole story :%

If you leave the accelerometer on the table and you let it measure it should give out (see its datasheet):
xout = 0g = 1.65V
yout = 0g = 1.65V
zout = -1g = 0.85V //not 1.65V in this position!
what I’m doing is that I’m recording a set of measures to calculate the average deviation from the value it should teoretically measure (aka 1.65V, 1.65V and 0.85V for x y z respectively). If you are familiar with statistical processes it is called standard deviation. Then, I’m subtracting these values from the measurements to eliminate the static error of the device: it is called auto-zero technique. This is the reason of the 0.85V down there.

Now, let’s say you dont implement autozero. If you convert the gvalue to unity vector you’ll get often something greater than 1 or less than -1. Why? Because of the static error of the device. Now you see this and you say “ok lets do autozero then”, good. The more sample you get to generate calValues[0], calValues[1] and calValues[2], the closer you get to have a unity vector. Since I dont want to get infinite samples, I have a residual error that translates to a unity vector that is not constrained from -1 to 1 but from -1.000002 to 1.04 lets say.

I decide I want to live with this very-small-error and constrain -1.000002 to be -1 and 1.04 to be 1.

This should clarify everything :slight_smile:

Yes, I understand what you're doing, but not why you are doing it that way - its not the obvious way from a geometric standpoint. And I don't think you realize that an accelerometer can't be calibrated in this way - you need to take measurements in different orientations (6 ideally) to characterize the zero-errors and scale-factors.

MarkT: Yes, I understand what you're doing, but not why you are doing it that way - its not the obvious way from a geometric standpoint. And I don't think you realize that an accelerometer can't be calibrated in this way - you need to take measurements in different orientations (6 ideally) to characterize the zero-errors and scale-factors.

@MarkT You're right, due to the device nonlinearities you need to calibrate the accelerometer in different positions and calibration holds only for small tilt angles. You can generate vectors from the code I wrote, but - to be honest, I dont care yet, it was just a test code... :roll_eyes: If you think its interesting, add it to the conversation to make it more complete, please. It will be helpful to everybody and I will learn your point of view.

Anyway, its clear you're not convinced and I cant convince you. But Freescale semiconductors can :) Read here http://www.freescale.com/webapp/sps/site/prod_summary.jsp?code=MMA7361LC&tab=Documentation_Tab&pspll=1&SelectedAsset=Documentation&ProdMetaId=PID/DC/MMA7361LC&fromPSP=true&assetLockedForNavigation=true&componentId=2&leftNavCode=1&pageSize=25&Documentation=Documentation/00210KscRcb%60%60Application%20Notes&fpsp=1&linkline=Application%20Notes. There are application notes on calibration, autozero and angle sensing and they do exactly what I do.