pitch and roll from 3-axis accelerometer

Forum 2005-2010 (read only) Software Syntax & Programs
1

Hi everyone!
I've been busy trying to figure out how to use my own cell phone (Sony Ericsson w910i) as a rc car control. For that purpose, I will use the embedded accelerometer and an arduino bt.
So far, so good. I've already made a java application for my w910i, that reads and sends the accelerometer data through bluetooth to the arduino bt.
My problem is: I'm not getting the desired results, when it comes to calculate the tilt of the accelerometer.
Here is my code:

#include <Servo.h>

Servo servo_1;
Servo servo_2;
char data[32] = {0};
int x, y, z;

void setup()
{
servo_1.attach(5);
servo_2.attach(6);
Serial.begin(115200);
}

void loop()
{
if(Serial.available() > 0)
{
for(int i = 0; i < sizeof(data) && Serial.available() > 0; i++)data = Serial.read();
_ Serial.flush();_

  • data[sizeof(data) - 1] = 0;*

  • sscanf(data, "%d %d %d", &x, &y, &z);*

  • int g_max = sqrt(sq(x) + sq(y) + sq(z));*

  • int a = map(x, -g_max, g_max, 0, 180);*

  • int b = map(y, -g_max, g_max, 0, 180);*

  • servo_1.write(a);*

  • servo_2.write(b);*

_ Serial.println(a);_
_ Serial.println(b);_

  • }*
    }
    [/quote]
    When the code gets executed, both servos don't stabilize. They keep changing their angles.
    EDIT: The accelerometer of the w910i is a ST LIS302DL
2

You probably want to look again at your receiver 'for' loop

(why would anyone be interested in the accelerometer type?)

3

I think there is no problem with the for-loop.
The problem is when I calculate "g_max", which is the length of the gravity vector.
It doesn't return a constant value.

4

I have some questions about your code.

First, is your carriage return broken? Put the action for the for loop inside curly braces, so that the body of the loop is clearly distinguishable.

 for(int i = 0; i < sizeof(data) && Serial.available() > 0; i++)
 {
    data[i] = Serial.read();
 }

Why does the loop exit? You have two possible reasons for the loop to end. One is that the array to hold the data filled up. The other is that there was no more serial data to read. Which is occurring in your case?

When you run out of serial data to read, have you received a full packet (and x value, a y value, and a z value)?

You have no idea whether the packet is complete, because you are not sending or looking for an end of packet marker.

Serial data transmission is slow. Reading serial data is fast. There is a very good chance that your for loop is reading all the data that has been received before all the data has been sent.

Do you have a clue what this statement does?

 Serial.flush();

It dumps any data in the serial buffer that has not yet been read. If your for loop ends because all the data has been read, this statement is useless. If your for loop ends because the array filled up, you have a problem that dumping random amounts of serial data will not solve. GET RID OF THIS STATEMENT. PERIOD.

    data[sizeof(data) - 1] = 0;

This puts a NULL at the very last position of the array, not after the last value just stored in the array. Whatever garbage was left in the array from the last pass through loop is still there, between the new data in the array and the end of the array.

My problem is: I'm not getting the desired results, when it comes to calculate the tilt of the accelerometer.

This may be because you are not sending valid data. If you fix the problems mentioned above, you will be receiving an x value, a y value and a z value. These appear to be position values, not vector values. The names i, j, and k typically imply vector data. If the data being sent IS a vector, you should use correct names in the Arduino code, or use some comments to explain what is being sent.

The problem is when I calculate "g_max", which is the length of the gravity vector.
It doesn't return a constant value.

If you are (trying to) send(ing) a unit vector, why is it necessary to compute the constant value on the Arduino, which is not ideally suited for performing floating point arithmetic? If it isn't a unit vector being sent, why are you surprised that the value is not constant?

Storing the output of a function that returns a floating point value in an int is an exercise in frustration. Are you frustrated yet?

5

Firstly, I was disappointed because the code did not perform the job I wanted.
Secondly, I'm sending the data from the mobile to arduino as set of 3 values separated by spaces, something like this: "x y z\n".
Thirdly, since I want arduino to understand the data and since I can't assure that the data arrives in one piece to the arduino, I have to clear the receive buffer. It's not funny when a set of illegible data arrives to arduino and forces him to crash.
When I refer to the length of the vector, I mean the norm of the vector, which is calculated using the Pythagorean theorem (Euclidean vector - Wikipedia);
Example:
g = x.ex + y.ey + z.ez
"g" is a vector;
"ex", "ey" and "ez" are direction vectors;
"x", "y" and "z" are numeric values.

This the new code:

#include <Servo.h>

Servo servo_1;
Servo servo_2;
char data[32] = {0};
double x, y, z;

void setup()
{
servo_1.attach(5);
servo_2.attach(6);
Serial.begin(115200);
}

void loop()
{
if(Serial.available() > 0)
{
delay(sizeof(data));
for(int i = 0; i < sizeof(data); i++)data = (Serial.read() % 0xff);

  • data[sizeof(data) - 1] = 0;*
    _ Serial.flush();_

_ char *ptr = data;_

  • x = strtod(ptr, &ptr);*

  • y = strtod(ptr, &ptr);*

  • z = strtod(ptr, &ptr);*

  • double g_max = sqrt(sq(x) + sq(y) + sq(z));*
    double a = (asin(x / g_max) * RAD_TO_DEG) + 90;
    double b = (asin(y / g_max) * RAD_TO_DEG) + 90;

  • servo_1.write(a);*

  • servo_2.write(b);*

Serial.println(g_max);
_ Serial.println(a);_
_ Serial.println(b);_

  • }*
    }
    [/quote]
    And now it works. Next step is smoothing.
6 
if(Serial.available() > 0)
  {
    delay(sizeof(data));
    for(int i = 0; i < sizeof(data); i++)data[i] = (Serial.read() % 0xff);

A delay based on the size of an array?
What happens when you change the bit rate?
Why not eliminate the delay and read data when it is actually there?

7

I keep using that statement. If I use it, arduino waits for the buffer to fill. If not, arduino reads strange values.
It's quite a mistery.
As PaulS mentioned:

Serial data transmission is slow. Reading serial data is fast. There is a very good chance that your for loop is reading all the data that has been received before all the data has been sent.

It's faster reading a byte from the receiver buffer than transmitting it, so a delay is needed.

8

No mystery, but not recommended.
It causes a delay of 32 milliseconds (and therefore limits your update rate to about 30Hz).
In those 32 milliseconds, you could have transmitted over 350 characters at 115200 bits per second

9

Where, in this new code, are you waiting for the end-of-packet marker (the \n)?
Suppose that the data sent looks like "0.0 0.0 1.0\n". The sender starts sending data.

You wait 32 microseconds for all the data to arrive. How did you decide that that was long enough? What happens if all the data arrives sooner than 32 microseconds?

How do you know that all the data has arrived, and only the data for one packet?

Suppose only 6 bytes makes it into the serial buffer before you start reading. You will read "0.0 0." and assume that you got a x value, a y value, and a z value.

The result will be x=0.0, y=0.0, and z=0.0. The value in g_max will also be 0.0.
Then, you divide by g_max. Oops.

While the code now SEEMS to work, it is certainly less than optimal.

Suppose that you send "0.7071 0.707 0.0\n" in one packet. You store this data in the data array. Now, suppose the next packet contains "1 0 0\n". The data array will now contain "1 0 010.707 0.0\n". When you parse this data, you will NOT get the correct results.

You are still possibly flushing random amounts of data. Suppose that while you are twiddling your thumbs, the sender sends "0 1 0\n1 0 0\n0.707 0.707 0\n". That's 3 full packets. You'll process the first packet, and discard the rest.

Suppose that the sender sent one and a half packets before the delay ended. You'll then throw that half packet away. The remaining packets will now be screwed up.

As I said earlier, it is essential that you get rid of this statement, and start paying attention to the end-of-packet marker.

10

You wait 32 microseconds for all the data to arrive

Not even close

11

I understand that the problem is when receiving the data. I don't have any idea how to implement a method that reads an entire line from serial, like the C# System.IO.Ports.SerialPort.ReadLine() method, which returns a line.
Can you show me a snippet of that?

12

I think I did suggest there was something wrong with the receive 'for' loop.

13

You wait 32 microseconds for all the data to arrive

Not even close

Hey, I was only off by 3 orders of magnitude. That's close.

14

I have to use the delay, or I will get junk data.
I'm sending "$ 18 36 1044\n"
without delay:
"ÿ ÿÿ1ÿ8ÿ ÿÿ3ÿ6ÿ ÿÿ1ÿ0ÿÿ4ÿ4ÿÿ"
with delay:
" 18 36 1044"

15

The "junk" is a -1 in ASCII. When there is no data to read, Serial.read() returns a -1. You accept that as valid data (it isn't) and store it in the array (and print it).

Delaying causes the microprocessor to nothing for a period of time except process interrupts. One of the interrupts that does get processed receives serial data.

Relying on delay to "ensure" that all the data has arrived will bite you in the posterior at some point.

16

Unless you are limiting the rate at which you send characters, your receive buffer could easily wrap twice in that 32 millisecond delay, and you'd never know.
Get rid of the delay.

17

I'm trying to use the String class, but the reference page is wrong about the String::getBytes() method.
It doesn't not return anything. It's a void.
"void String::getBytes(unsigned char*, unsigned int)"

This is the last approach of my code:

#include <Servo.h>

Servo servo_1;
Servo servo_2;
char data[32] = {0};
double x, y, z;
double g_max;
double a, b;

void setup()
{
servo_1.attach(5);
servo_2.attach(6);
Serial.begin(115200);
}

void loop()
{
if(Serial.available() > 0 && Serial.read() == '<')
{
int i = 0;
while(Serial.peek() != '>')
{
if(Serial.peek() != EOF)
{
data[i++] = Serial.read();
}
}
data* = 0;*
_ char *ptr = data;_

  • x = strtod(ptr, &ptr);*
  • y = strtod(ptr, &ptr);*
  • z = strtod(ptr, &ptr);*
  • g_max = sqrt(sq(x) + sq(y) + sq(z));*
  • if(g_max != 0.0)*
  • {*
    a = (asin(x / g_max) * RAD_TO_DEG);
    b = (asin(y / g_max) * RAD_TO_DEG);
  • }*
  • }*
  • servo_1.write(a + 90);*
  • servo_2.write(b + 90);*
    }
    [/quote]