UAV, reading sensor data

A long time before we did our session where we controlled some servos, we had a session where we were reading sensor data.

We’ve tried this a number of times, but this instance was where we actually started to get worthwhile data, and we could interpret it correctly.

We are currently reading two sensors to find orientation data: the 3 axis gyroscope and the 3 axis accelerometer.

I’ll cover the accelerometer first.

Reading Accelerometer Data

We’re using a 3 axis accelerometer  which if read correctly can tell us the pitch and roll of the aircraft (actually, just the pitch and roll of the sensor, but the sensor will be built into the aircraft).

Each axis in the accelerometer (we’ll call them accX, accY and accZ) tells us how pull that sensor is experiencing (at rest, this is due to gravity). We only need 1 axis to start with, to measure Pitch.

In this crappy diagram, the rectangle represents our accelerometer. It’s measuring apparent gravity. As the acceleromter has its pitch increased (rotated clockwise, in this case), the apparent gravity being measured in that axis will decrease.

Theta represents the angle between the apparent gravity and actual gravity.

The trigonometery to calculate pitch is pretty easy:

apparent gravity = cos (theta) * g

or:

acc = cos (theta) * g

The relationship between theta and pitch is very simple: simply add 90 degrees to theta to get pitch.

So, rewrite

apparent gravity = cos (theta) * g 

as

theta = acos (acc / g)

And then to get the actual pitch:

pitch = asin (acc / g)

/*------------------------------------------------------------------------*/
//Accellerometer Read and Output Section
 
  float xAccRate, yAccRate, zAccRate;
  double pitch, roll;
 
  xAccRate = (accReadX());
  yAccRate = (accReadY());
  zAccRate = (accReadZ());
 
  double measured_g = sqrt((xAccRate*xAccRate)+(yAccRate*yAccRate)+(zAccRate*zAccRate));
 
  roll = (atan2(xAccRate/128,zAccRate/128))*(180/3.141);
  pitch = (atan2(yAccRate/128,zAccRate/128))*(180/3.141);

Reading Gyro Data

Gyro data is actually a lot easier to read, in some ways. All a the gyrometer is doing is measuring rate of change since the last reading.

Therefore, all we need to do is integrate over a period of time to get the rotation in a certain axis.

Code:

/*-----------------------------------------------------------------------------*/
//Gyro Read & Output Section
 
  //Create variables for outputs
  float xGyroRate, yGyroRate, zGyroRate, xGyroRate2, yGyroRate2, zGyroRate2;
  long Time;
  //Read the x,y and z output rates from the gyroscope & correct for some innacuracy; convert to seconds
  xGyroRate = (gyroReadX())/57.5;
  yGyroRate = (gyroReadY())/57.5;
  zGyroRate = (gyroReadZ())/57.5;
  //Determine how long it's been moving at this 'rate', in seconds
  Time = (millis()-previousMillis);
  //Multiply rates by duration
  xGyroRate2 = -(xGyroRate/Time)/4;
  yGyroRate2 = -(yGyroRate/Time)/4;
  zGyroRate2 = -(zGyroRate/Time)/4;
  //Add to cumulative figure
  if (((xGyroRate2)>(gyroLPF))||((xGyroRate2)<(-gyroLPF)))   CumulatGyroX += (xGyroRate2);   if (yGyroRate2>gyroLPF||yGyroRate2<-gyroLPF)   CumulatGyroY += (yGyroRate2);   if (zGyroRate2>gyroLPF||zGyroRate2<-gyroLPF)
  CumulatGyroZ += (zGyroRate2);

Pretty simple 🙂

Some pretty graphs

Of course, that code took us a long time to actually write. And we may have borrowed some from elsewhere, I can’t for the life of me remember. Once we’d got it working, we plumbed the serial output into GNUPlot, so that we could get a visual representation of what was going on.

The general idea was to tilt the board by 90 degrees in once axis, then return it to it’s original position. This would produce a sine wave, eg:

After mucking about for a bit with minicom, I used this command to capture the serial output:

sudo minicom --capture mincap

The serial output just looks something like this:

ID: 69
2 0 0
-32 -4 -27
-1 -1 1
0 -1 1
0 0 1
0 -1 0
0 1 0
0 1 0
1 -1 0
1 -1 0
0 -2 0
0 -1 0

Once we’d got that, I spend yet more time mucking around in gnuplot. Actually, I jest. It was pretty easy to plot a graph.

The first graph we got was this:

We’d done something very obvious and simple wrong.

Spot the deliberate mistake.

Upon seeing this, we retired to the pub. Over a pint or three, we realised what it was we’d done wrong.

The Arduino board has an integer size of 16 bit, meaning it has a max size of 32,767. If you try to do

int i = 32767 + 1;

then i would have the value -32767.

And that’s exactly what was happening. We’d used an Integer, because occasionally I’m as thick as a brick sandwich.

We weren’t doing anything with the data yet to convert it to degrees, and we weren’t even sure how big the numbers would be, and we were just adding it all up.

Here’s another graph showing the same thing, but with the dots joined up:

After we started using the (entirely more sensible) long data type instead, we got what we’d originally expected:

So, as predicted, we got ourselves half a sine wave. If we tilt it by 90 degrees in one direction, then back to flat, then 90 degrees in the other direction, we get the full sine wave:

If the graph is a bit lumpy, it’s because Matt had the shakes.

In the next post, I’ll talk about working out the actual orientation, and sensor drift.  We’re also going to compare the output from the accelerometer and gyro.