Other designs


Playing with an Arduino and sensors

Eventually, this will become a design for a stabiliser for an RC helicopter (roll and pitch). I will describe the stages I went through designing and building it, but don't hold your breath for updates.

If you spot errors, or think I'm casually skipping over important bits, please let me know.

Contents:

  1. The hardware
  2. First software: reading the IMU data
  3. Getting rid of zero bias, calculating angles for pitch and roll, and a mysterious foo factor
  4. The mathematically painful bit: the Kalman filter
  5. Intermezzo: an idea for an artificial horizon
  6. What about yaw?
  7. Another intermezzo: alternative hardware
  8. Vertical Acceleration Rejection
  9. Cuidado! Lama!
  10. The controller
  11. Some filtering required

The hardware

I ordered an Arduino Nano and an IMU Digital Combo Board (with an ITG-3200 and an ADXL345 on it) from http://www.freeduino.eu/. Nice enough people, but their logistics need some work; it took quite a while to get the stuff.

After connecting the Arduino to a USB port on my PC, I installed the development environment, which, of course, wouldn't start at first. It turned out my bluetooth cell phone, mapped to a COM port on my PC, made the software time out and exit. With the phone problem solved, things worked fine and I made a LED blink on the Arduino (w00t!) using the example software. So far, so good.

IMU board connected

Next, I connected the IMU board to the Arduino. Only 4 wires, 2 for the I2C (A4 and A5 on the Arduino Nano), ground, and 3.3V. The Arduino Nano gives you 3.3V, as long as it is being powered over USB, so I used that to supply the IMU board with the gyros and accelerometers.

A thing to watch out for here is that the chips on the IMU board are 3.3V, and the Arduino Nano is 5V, which is a potential problem for I2C communication. The 3.3V logic "1" is enough to be seen as a "1" by the 5V logic of the Arduino, but pulling up the I2C lines to 5V would mean using the IMU board beyond spec.

I disconnected the I2C again and measured the voltage levels on the Arduino Nano's I2C lines while running some dummy I2C software on it, and to my surprise the I2C lines showed 5V without my having installed pull-up resistors.

It turns out the wire library by default activates the Arduino's internal pull-ups. I consider that unwanted behaviour, so I changed the libraries (look in the twi.c file and search for "pull-up" to find the lines you need to comment out). The IMU board has pull-up resistors installed, and with the I2C bus now safely operating at 3.3V, all was well.

top

First software: reading the IMU data

To get the gyros and accelerometers on the IMU board going, you really need to know what's in the datasheets that come with the chips on the IMU board, especially the register definitions. Some settings required, see the code:

#include <Wire.h> //The I2C library
        
int gyroResult[3], accelResult[3];

//Function for writing a byte to an address on an I2C device
void writeTo(byte device, byte toAddress, byte val) {
  Wire.beginTransmission(device);  
  Wire.send(toAddress);        
  Wire.send(val);        
  Wire.endTransmission();
}

//Function for reading num bytes from addresses on an I2C device
void readFrom(byte device, byte fromAddress, int num, byte result[]) {
  Wire.beginTransmission(device);
  Wire.send(fromAddress);
  Wire.endTransmission();
  Wire.requestFrom((int)device, num);
  int i = 0;
  while(Wire.available()) {
    result[i] = Wire.receive();
    i++;
  }
}

//Function for reading the gyros.
void getGyroscopeReadings(int gyroResult[]) {
  byte buffer[6];
  readFrom(0x68,0x1D,6,buffer);
  gyroResult[0] = (((int)buffer[0]) << 8 ) | buffer[1]; //Combine two bytes into one int
  gyroResult[1] = (((int)buffer[2]) << 8 ) | buffer[3];
  gyroResult[2] = (((int)buffer[4]) << 8 ) | buffer[5];
} 

//Function for reading the accelerometers
void getAccelerometerReadings(int accelResult[]) {
  byte buffer[6];
  readFrom(0x53,0x32,6,buffer);
  accelResult[0] = (((int)buffer[1]) << 8 ) | buffer[0]; //Yes, byte order different from gyros'
  accelResult[1] = (((int)buffer[3]) << 8 ) | buffer[2];
  accelResult[2] = (((int)buffer[5]) << 8 ) | buffer[4];
}

void setup() {
  Wire.begin();            //Open I2C communications as master
  Serial.begin(115200);    //Open serial communications to the PC to see what's happening
  
  writeTo(0x53,0x31,0x09); //Set accelerometer to 11bit, +/-4g
  writeTo(0x53,0x2D,0x08); //Set accelerometer to measure mode
  writeTo(0x68,0x16,0x1A); //Set gyro to +/-2000deg/sec and 98Hz low pass filter
  writeTo(0x68,0x15,0x09); //Set gyro to 100Hz sample rate
}

void loop() {
  getGyroscopeReadings(gyroResult);
  getAccelerometerReadings(accelResult);

  Serial.print(gyroResult[0]);
  Serial.print("\t");
  Serial.print(gyroResult[1]);
  Serial.print("\t"); 
  Serial.print(gyroResult[2]);
  Serial.print("\t\t");
  Serial.print(accelResult[0]);
  Serial.print("\t");
  Serial.print(accelResult[1]);
  Serial.print("\t");
  Serial.print(accelResult[2]);
  Serial.print("\n");

  delay(50);
}

Using the serial monitor in the development environment, this showed me 6 columns of scrolling numbers, that changed when I moved the breadboard with the IMU board on it. I put that down as a success well beyond making a LED blink. The left-hand three columns are the X-, Y- and Z-axis outputs of the gyros; the right-hand three are ditto for the accelerometers.

zero bias errors

The numbers were meaningless, not having been converted to SI-units, but it was immediately clear (see screenshot on the left) that both the gyros and the accelerometers had bias errors; they didn't show 0 when the IMU board was stationary, as they should, except for the Z-axis accelerometer value, of course, what with gravity and all. Also, the axes of the gyros and accelerometers didn't seem to match, but that is easily corrected.

top

Getting rid of zero bias, calculating angles for pitch and roll, and a mysterious foo factor

I decided to get rid of zero bias by taking 50 measurements, averaging the results, and using this to compensate. I put in code for calculating angles (in degrees) from the gyro and accelerometer readings.

Foo!

Plotting the data after running the code, I saw (shock! horror!) in the plots of the data that the angles calculated from the gyro readings by integrating were falling a factor of about 1.2 short of the angles from the accelerometers.

I first decided to add a foo factor of 1.2 to the program, and that worked well, but later and closer reading of the datasheet revealed that it wasn't simply a matter of dividing 4000deg/s into the largest 16 bits number giving 65535/4000=16.38375 as a conversion factor between the register values and degrees/second, but 14.375. Don't know why this is, but there you have it. The foo factor should have been 1.14, but we can do without it now.

The code now becomes:

#include <Wire.h>

int gyroResult[3], accelResult[3];

float timeStep = 0.02;          //20ms. Need a time step value for integration of gyro angle from angle/sec
float biasGyroX, biasGyroY, biasGyroZ, biasAccelX, biasAccelY, biasAccelZ;
float pitchGyro = 0;
float pitchAccel = 0;
float rollGyro = 0;
float rollAccel = 0;
unsigned long timer;

void writeTo(byte device, byte toAddress, byte val) {
  Wire.beginTransmission(device);  
  Wire.send(toAddress);        
  Wire.send(val);        
  Wire.endTransmission();
}

void readFrom(byte device, byte fromAddress, int num, byte result[]) {
  Wire.beginTransmission(device);
  Wire.send(fromAddress);
  Wire.endTransmission();
  Wire.requestFrom((int)device, num);
  int i = 0;
  while(Wire.available()) {
    result[i] = Wire.receive();
    i++;
  }
}

void getGyroscopeReadings(int gyroResult[]) {
  byte buffer[6];
  readFrom(0x68,0x1D,6,buffer);
  gyroResult[0] = (((int)buffer[0]) << 8 ) | buffer[1];
  gyroResult[1] = (((int)buffer[2]) << 8 ) | buffer[3];
  gyroResult[2] = (((int)buffer[4]) << 8 ) | buffer[5];
} 

void getAccelerometerReadings(int accelResult[]) {
  byte buffer[6];
  readFrom(0x53,0x32,6,buffer);
  accelResult[0] = (((int)buffer[1]) << 8 ) | buffer[0];
  accelResult[1] = (((int)buffer[3]) << 8 ) | buffer[2];
  accelResult[2] = (((int)buffer[5]) << 8 ) | buffer[4];
}

void setup() {
  int totalGyroXValues = 0;
  int totalGyroYValues = 0;
  int totalGyroZValues = 0;
  int totalAccelXValues = 0;
  int totalAccelYValues = 0;
  int totalAccelZValues = 0;
  int i;
  
  Wire.begin(); 
  Serial.begin(115200);
  
  writeTo(0x53,0x31,0x09); //Set accelerometer to 11bit, +/-4g
  writeTo(0x53,0x2D,0x08); //Set accelerometer to measure mode
  writeTo(0x68,0x16,0x1A); //Set gyro to +/-2000deg/sec and 98Hz low pass filter
  writeTo(0x68,0x15,0x09); //Set gyro to 100Hz sample rate
  
  // Determine zero bias for all axes of both sensors by averaging 50 measurements
  delay(100); //wait for gyro to "spin" up
  for (i = 0; i < 50; i += 1) {
    getGyroscopeReadings(gyroResult);
    getAccelerometerReadings(accelResult);
    totalGyroXValues += gyroResult[0];
    totalGyroYValues += gyroResult[1];
    totalGyroZValues += gyroResult[2];
    totalAccelXValues += accelResult[0];
    totalAccelYValues += accelResult[1];
    totalAccelZValues += accelResult[2];
    delay(50);
  }
  biasGyroX = totalGyroXValues / 50;
  biasGyroY = totalGyroYValues / 50;
  biasGyroZ = totalGyroZValues / 50;
  biasAccelX = totalAccelXValues / 50;
  biasAccelY = totalAccelYValues / 50;
  biasAccelZ = (totalAccelZValues / 50) - 256; //Don't compensate gravity away! We would all (float)!
}

void loop() {
  timer = millis(); //get a start value to determine the time the loop takes
  getGyroscopeReadings(gyroResult);
  getAccelerometerReadings(accelResult);
  
  //Calculate the pitch in degrees as measured by the accelerometers. Note that 8g into 11 bits gives 256 bits/g
  pitchAccel = atan2((accelResult[1] - biasAccelY) / 256, (accelResult[2] - biasAccelZ) / 256) * 360.0 / (2*PI);
  //Calculate the pitch in degrees as measured by the gyros, angle = angle + gyro reading * time step.
  pitchGyro = pitchGyro + ((gyroResult[0] - biasGyroX) / 14.375) * timeStep;
  //ditto for roll
  rollAccel = atan2((accelResult[0] - biasAccelX) / 256, (accelResult[2] - biasAccelZ) / 256) * 360.0 / (2*PI);
  rollGyro = rollGyro - ((gyroResult[1] - biasGyroY) / 14.375) * timeStep; 
  
  Serial.print(pitchGyro);
  Serial.print("\t");
  Serial.print(pitchAccel);
  Serial.print("\t");
  Serial.print(rollGyro);
  Serial.print("\t");
  Serial.print(rollAccel);
  Serial.print("\n");
  
  timer = millis() - timer;          //how long did the loop take?
  timer = (timeStep * 1000) - timer; //how much time to add to the loop to make it last time step msec
  delay(timer);                      //make one loop last time step msec
}
angle plot

If you plot the data from the serial output (see the plot on the left), you will notice that the gyros drift over time (made worse by integrating), and that the accelerometer data doesn't drift, but is quite noisy (the smoothness of the gyro curves proves that the noise isn't caused by a drinking problem of the person manipulating the breadboard, as one reader suggested).

Having solved the foo factor problem, the angles as calculated from both types of sensors overlap very nicely.

We need some real maths now to get useful and stable values by combining the data from both types of sensor. Ideally, the gyros should be "right" in the short run, and the accelerometers in the long run. Visually, you want the curves to be shaped like the gyro output, but have the values of the accelerometer output.

Also, even after a long time and a lot of drift from the gyros, all the drift should be filtered away to leave the average value of the accelerometer output, that, as you can see on the right-hand side of the plot, goes back to 0 nicely when there is no pitch or roll, where the pitch gyro is obviously off by a few degrees.

top

The mathematically painful bit: the Kalman filter

I don't understand Kalman filtering yet, so I can't explain it here. That's a painful admission. I took the algorithm from code I found in a blog by Ian Glover. It works perfectly, but I don't know why. If you have, or know of, a good explanation, please let me know.

After integrating the Kalman filter algorithm into the code, I got this:

#include <Wire.h>

int gyroResult[3], accelResult[3];
float timeStep = 0.02;
float biasGyroX, biasGyroY, biasGyroZ, biasAccelX, biasAccelY, biasAccelZ;
float pitchGyro = 0;
float pitchAccel = 0;
float pitchPrediction = 0; //Output of Kalman filter
float rollGyro = 0;
float rollAccel = 0;
float rollPrediction = 0;  //Output of Kalman filter
float giroVar = 0.1;
float deltaGiroVar = 0.1;
float accelVar = 5;
float Pxx = 0.1; // angle variance
float Pvv = 0.1; // angle change rate variance
float Pxv = 0.1; // angle and angle change rate covariance
float kx, kv;
unsigned long timer;

void writeTo(byte device, byte toAddress, byte val) {
  Wire.beginTransmission(device);  
  Wire.send(toAddress);        
  Wire.send(val);        
  Wire.endTransmission();
}

void readFrom(byte device, byte fromAddress, int num, byte result[]) {
  Wire.beginTransmission(device);
  Wire.send(fromAddress);
  Wire.endTransmission();
  Wire.requestFrom((int)device, num);
  int i = 0;
  while(Wire.available()) {
    result[i] = Wire.receive();
    i++;
  }
}

void getGyroscopeReadings(int gyroResult[]) {
  byte buffer[6];
  readFrom(0x68,0x1D,6,buffer);
  gyroResult[0] = (((int)buffer[0]) << 8 ) | buffer[1];
  gyroResult[1] = (((int)buffer[2]) << 8 ) | buffer[3];
  gyroResult[2] = (((int)buffer[4]) << 8 ) | buffer[5];
} 

void getAccelerometerReadings(int accelResult[]) {
  byte buffer[6];
  readFrom(0x53,0x32,6,buffer);
  accelResult[0] = (((int)buffer[1]) << 8 ) | buffer[0];
  accelResult[1] = (((int)buffer[3]) << 8 ) | buffer[2];
  accelResult[2] = (((int)buffer[5]) << 8 ) | buffer[4];
}

void setup() {
  int totalGyroXValues = 0;
  int totalGyroYValues = 0;
  int totalGyroZValues = 0;
  int totalAccelXValues = 0;
  int totalAccelYValues = 0;
  int totalAccelZValues = 0;
  int i;
  
  Wire.begin(); 
  Serial.begin(115200);
  
  writeTo(0x53,0x31,0x09); //Set accelerometer to 11bit, +/-4g
  writeTo(0x53,0x2D,0x08); //Set accelerometer to measure mode
  writeTo(0x68,0x16,0x1A); //Set gyro to +/-2000deg/sec and 98Hz low pass filter
  writeTo(0x68,0x15,0x09); //Set gyro to 100Hz sample rate
  
  delay(100); //wait for gyro
  for (i = 0; i < 50; i += 1) {
    getGyroscopeReadings(gyroResult);
    getAccelerometerReadings(accelResult);
    totalGyroXValues += gyroResult[0];
    totalGyroYValues += gyroResult[1];
    totalGyroZValues += gyroResult[2];
    totalAccelXValues += accelResult[0];
    totalAccelYValues += accelResult[1];
    totalAccelZValues += accelResult[2];
    delay(50);
  }
  biasGyroX = totalGyroXValues / 50;
  biasGyroY = totalGyroYValues / 50;
  biasGyroZ = totalGyroZValues / 50;
  biasAccelX = totalAccelXValues / 50;
  biasAccelY = totalAccelYValues / 50;
  biasAccelZ = (totalAccelZValues / 50) - 256;
  
  Serial.print("Pitch gyro\tPitch accel\tPitch Kalman\t");
  Serial.print("Roll gyro\tRoll accel\tRoll Kalman\n");
}

void loop() {
  timer = millis();
  getGyroscopeReadings(gyroResult);
  getAccelerometerReadings(accelResult);
  
  pitchAccel = atan2((accelResult[1] - biasAccelY) / 256, (accelResult[2] - biasAccelZ) / 256) * 360.0 / (2*PI);
  pitchGyro = pitchGyro + ((gyroResult[0] - biasGyroX) / 14.375) * timeStep;
  pitchPrediction = pitchPrediction + ((gyroResult[0] - biasGyroX) / 14.375) * timeStep;
  
  rollAccel = atan2((accelResult[0] - biasAccelX) / 256, (accelResult[2] - biasAccelZ) / 256) * 360.0 / (2*PI);
  rollGyro = rollGyro - ((gyroResult[1] - biasGyroY) / 14.375) * timeStep; 
  rollPrediction = rollPrediction - ((gyroResult[1] - biasGyroY) / 14.375) * timeStep;
  
  Pxx += timeStep * (2 * Pxv + timeStep * Pvv);
  Pxv += timeStep * Pvv;
  Pxx += timeStep * giroVar;
  Pvv += timeStep * deltaGiroVar;
  kx = Pxx * (1 / (Pxx + accelVar));
  kv = Pxv * (1 / (Pxx + accelVar));
  
  pitchPrediction += (pitchAccel - pitchPrediction) * kx;
  rollPrediction += (rollAccel - rollPrediction) * kx;
  
  Pxx *= (1 - kx);
  Pxv *= (1 - kx);
  Pvv -= kv * Pxv;
  
  Serial.print(pitchGyro);
  Serial.print("\t");
  Serial.print(pitchAccel);
  Serial.print("\t");
  Serial.print(pitchPrediction);
  Serial.print("\t"); 
  Serial.print(rollGyro);
  Serial.print("\t");
  Serial.print(rollAccel);
  Serial.print("\t");
  Serial.print(rollPrediction);
  Serial.print("\n");
  
  timer = millis() - timer;
  timer = (timeStep * 1000) - timer; 
  delay(timer);
}

Here's a plot of the data. It works so well it makes your eyes water. I now have pitch and roll data to Do Things With.

Kalman filter output

top

Intermezzo: an idea for an artificial horizon

Connecting two gyros to the Arduino, one for pitch- and one for roll "display" could be the basis for an artificial horizon, one that has a small model airplane show the roll and pitch of the actual plane it is in. Just a few extra lines of code required.

top

What about yaw?

Most RC helicopters come with a yaw gyro (which is usually more than just a gyro), so I am not all that interested about including it in the design.

There is, however, yaw information to be had from the hardware I use; information from one gyro, to be exact. The accelerometers don't notice yaw, unless they are placed outside of the rotation axis. Placing them outside the rotation axis would complicate the other calculations, and I just can't be bothered.

Yaw rate
The graph shows the information we do have. Note that this shows the angular velocity in degrees/second, without integration over time to arrive at angle data. It starts at 0 degrees/second, and goes back to that after my having stopped my RSI-causing manipulations of the breadboard. That's hopeful; this information by itself could be used to stabilise yaw, at least to some extent. Now, let's try integrating over these values.
Yaw angle
And there we have it: the Dreaded Drift. This means that the angle as derived from the gyro data can't be trusted. Also, there is no way to compensate for this drift by using other data from this hardware. This means we would need to add an electronic compass or magnetometer to get reliable yaw angles. Looks doable, but I'm not going to. Maybe later.

Concluding we could say that there is information to correct short-term disturbances in the yaw, but a long-term heading hold isn't in the cards with this hardware.

top

Another intermezzo: alternative hardware

I just realised that all the hardware you need for this project, plus an electronic compass, a video camera, an internet connection, a loudspeaker, a USB port, Bluetooth, loads of processing power and a proper operating system and SDK are all available in my Samsung Galaxy S II telephone. It could be fun, when everything works, to port the software to the telephone and use that. With everything switched on it will probably drain its own and the helicopter's batteries in 5 minutes, but it's still a lot of functionality in a 116 grams package.

top

Vertical Acceleration Rejection

Now isn't that a nice term? I don't mean "unwillingness to crash into the ground", but this: as you can see in the code, the data from the Z-axis accelerometer is used in calculating both pitch- and roll angles. The problem is, that not only the angle of this accelerometer to the gravity field influences its reading, but also vertical acceleration. Accelerating downwards, for instance, would give a reading of less than 1g, suggesting that the accelerometer is at an angle when it is not.

So, some more antics with the breadboard are in order to see what happens when the breadboard is accelerated downwards, or dropped, as some say.

Vertical acceleration test

Well, doesn't look too bad at first glance. The readings from the accelerometers are large, as expected, off the scale, even, but that may have to do with my setting the scale in such a way that I could use the term "off the scale".

As you can see the output of the Kalman filter is far less nervous, and is only "pulling away" a little bit, a few degrees, from the gyro readings. Note that this was a short period of acceleration, just long enough to hurt my knuckles on the floor. The Kalman output will start following the accelerometer output more if the acceleration is sustained over a longer period.

The same problem exists for other axes of acceleration, of course. Question is, does this need fixing and if so, what to do about it? I'm going to wait until there's a flying prototype to look into this further.

top

Cuidado! Lama!

There it is, dismembered but fully functional: a nice Lama helicopter for experimenting (thanks, Wessel!). It's a coaxial, two rotor, no tail rotor design, and it comes with a heading hold gyro and a mixer for throttle and rudder (and gyro). So, no yaw problems. It should be fairly stable as-is, or at least that is what I was told.

Swashplate Servos

It has fixed-pitch rotors, and the bottom one is controlled by a swashplate that tilts the whole rotor. The swashplate is driven by two servos. The top rotor is stabilised by a flybar, the thingy lying in the foreground in the next picture.

The beautiful classic portable oscilloscope in the background (thanks, Simon!) is showing the 1ms pulse being sent to the throttle, setting it to zero. Safety first here: the rotor blades can do real damage, so it is best to cut the power or at least set the throttle to zero before you come near the helicopter.

Disclaimer aimed at our friends from the colonies: if you let the blades cut you, it's your own fault, don't blame me, go put a dog in a microwave if you need cash.

Umbilical

For testing the helicopter, I made an umbilical cord and connected the servos, the throttle and rudder to the Arduino, and added a 50K potmeter to set the power going to the rotor.

Note that the test software makes the servo's arms move and lets you set the rotor speed, but the rotor speed is updated only every few seconds, so there is no instant throttle response in this test: don't get caught unawares by the spinning blades.

Also note that the helicopter will lift off in this set-up when given enough power.

A final note: the gyro/mixer thingy the Lama came with needs some time to initialise, and it will only initialise properly when throttle is set to zero.

A final final note: don't miss the coffee stains in the foreground, I made them myself.

Here's the code used for testing:

#include <Servo.h> 
 
Servo leftServo, rightServo, throttleServo, rudderServo;
int angle = 90;
int i, val;
 
void setup() { 
  leftServo.attach(5, 600, 2400);
  rightServo.attach(4, 600, 2400);
  throttleServo.attach(3, 600, 2400);
  rudderServo.attach(2, 600, 2400);
  //Make a 0 and a 5V for the potmeter
  pinMode(A0, OUTPUT);
  digitalWrite(A0, HIGH);
  pinMode(A2, OUTPUT);
  digitalWrite(A2, LOW);
  //A1 is where the wiper of the potmeter connects
  pinMode(A1, INPUT);
  //Set swashplate servos to 90 degrees ("arms wide" as in the picture)
  leftServo.write(90);
  rightServo.write(90);
  delay(50);
}
 
void loop() {
  val= analogRead(1);              //Get the potmeter value
  val = map(val, 0, 1023, 0, 179); //Map the potmeter value to 1-179 degrees
  throttleServo.write(val);        //Set the throttle
  rudderServo.write(90);           //Set the rudder to the middle position

  //Workout program for the servos
  for (i = 1; i >= 0; i--) {
    throttleServo.write(val);
    leftServo.write(angle);
    rightServo.write(angle);    
    delay(300);
    leftServo.write(angle+30);   
    rightServo.write(angle-30);  
    delay(300);
    leftServo.write(angle-30);
    rightServo.write(angle+30);    
    delay(300);    
  }
  for (i = 1; i >= 0; i--) {
    throttleServo.write(val);
    leftServo.write(angle);
    rightServo.write(angle);    
    delay(300);
    leftServo.write(angle+30);   
    rightServo.write(angle+30);  
    delay(300);
    leftServo.write(angle-30);
    rightServo.write(angle-30);    
    delay(300);    
  }
  
} 

Everything seemed to work well, and the helicopter did want to take off when given enough power. It felt a bit funny holding it when under power, because the constant swashplate movements were making it pull in different directions.

I was told that on a Lama of this type one servo is for pitch, and the other for roll. Driving the servos individually like that, however, didn't seem to drive the swashplate properly, always showing both pitch and roll, and giving only a feeble response.

I noticed during the servo workout (seen from the front the code above really makes it look like a workout) that driving both servos at the same time, both up/down for pitch, and one up, one down for roll, gave a much better and cleaner response of the swashplate. I decided to go with the latter option, even though it is a bit more complex. It does have an advantage: the servos share the workload. Maybe a real Lama connoisseur can explain what's what and why here, I'm just going with what I see happening for now.

top

The controller

After mounting the gyro and accelerometer board on the helicopter, the only thing missing now is a controller that drives the servos, in response to both the Kalman filter output (the actual angles) and the user input on the transmitter (the desired angles). No user input, i.e. stick centered, should result in the helicopter's staying horizontal.

Because I am impatient, the first effort will be a "naive" controller, that simply drives both servos to tilt the swashplate away from the helicopter's pitch and roll in the direction of zero pitch and roll, using the amount of degrees of deviation, multiplied by a factor, to set the servo angels. For a combination of pitch and roll, the calculated servo angle settings are added up.

Note that adding up the servo arm angles could result in driving the servos beyond their maximum angles, so the values here really should be maximised, or mapped to a curve, but as I said, I am impatient.

In code, this approach would look something like this:

//Controller
float factor = 3; //speed of reaction to deviation from 0 pitch and roll, higher number = more aggressive response
float leftCalc, rightCalc;
leftCalc = 90.0 - factor * (pitchPrediction - rollPrediction);
rightCalc = 90.0 + factor * (pitchPrediction + rollPrediction);
leftServo.write((int)leftCalc);
rightServo.write((int)rightCalc);

Being a bit less impatient (coffee must be wearing off), I added a 10-tap FIR filter to smooth the servos' response and reject some noise.

I also limited the throw of the servo arms to between 50 and 130 degrees. Here are the main bits I added to the code:

const int FIRArrayLength = 10;
float leftServoFIRArray[FIRArrayLength];
float rightServoFIRArray[FIRArrayLength];
Servo leftServo, rightServo;

float FIR(float value, float array[]) {
  int i;
  float weighedArrayTotal = 0;
  float tapFactor = 0;
  float divider = 0;
  for(i = FIRArrayLength-1; i > 0; i--){
    array[i] = array[i-1];
  }
  array[0] = value;
  for(i = FIRArrayLength-1; i >= 0; i--){
    //tapFactor = 1-(float)i/FIRArrayLength; //Decreases factors towards past
    tapFactor = 1;                           //Simple moving average
    divider += tapFactor;
    weighedArrayTotal = weighedArrayTotal + array[i] * tapFactor; 
  }
  return weighedArrayTotal/divider;
} 

leftServo.attach(5, 600, 2400);
rightServo.attach(4, 600, 2400);
  
//Controller
float pFactor = 3;
float minServoAngle = 50;
float maxServoAngle = 130;
float pitch, roll, leftCalc, rightCalc, leftCalcFIR, rightCalcFIR, leftServoSignal, rightServoSignal;
pitch = pitchPrediction;
roll = rollPrediction;
leftCalc = pitch - roll;
rightCalc = pitch + roll;
leftCalcFIR = FIR(leftCalc, leftServoFIRArray);
rightCalcFIR = FIR(rightCalc, rightServoFIRArray);
leftServoSignal = 90.0 - (pFactor * leftCalcFIR);
rightServoSignal = 90.0 + (pFactor * rightCalcFIR);
if (leftServoSignal > maxServoAngle) {leftServoSignal = maxServoAngle;};
if (leftServoSignal < minServoAngle) {leftServoSignal = minServoAngle;};
if (rightServoSignal > maxServoAngle) {rightServoSignal = maxServoAngle;};
if (rightServoSignal < minServoAngle) {rightServoSignal = minServoAngle;};
leftServo.write((int)round(leftServoSignal));
rightServo.write((int)round(rightServoSignal));

Nothing is free: using a 10 taps FIR filter will slow the response a little, and will increase the calculation time in the loop from about 4ms to about 7ms, but that is still well within 20ms, the normal update rate for servos.

Video removed because of bandwidth abuse
by d-69-91-146-143.dhcp4.washington.edu,
d-69-91-182-81.dhcp4.washington.edu and
50-46-185-254.evrt.wa.frontiernet.net

Manipulating the helicopter with no engines running, the controller above shows the intended behaviour. Fingers crossed for what the shake, rattle and roll of running engines will do to the sensors and algorithms.

top

Some filtering required

Grass

As it turns out, the moving parts of the helicopter generate a lot of noise on the sensor readings. This noise is not random and not symmetrically dirstributed around zero; it's a mess, like unkempt grass growing on the data plots. If you look at the picture on the left, you'll see what I mean.

The white and yellow lines show the signals going to the servos. Just gunning the engines to maximum power will give huge deviations from "flat" even when the helicopter is stationary.

As I don't understand Kalman filters yet, I decided to try some filtering (FIR and IIR) on the raw sensor data before they go into the Kalman filter. The code I'm using to experiment with this:

#include <Wire.h>
#include <Servo.h> 

int gyroResult[3], accelResult[3];        //Store raw sensor output
int loopsPerServoUpdate = 2;              //Update servos once every loopsPerServoUpdate loops
int loopCounter = 0;                      //Used for driving servos once every loopsPerServoUpdate loops
float timeStep = 0.01;                    //Main loop should run at 1/timeStep Hz
float biasGyroX, biasGyroY, biasGyroZ;    //Bias values
float biasAccelX, biasAccelY, biasAccelZ; //for bias corrections
float gyroX, gyroY, gyroZ;                //Values after bias
float accelX, accelY, accelZ;             //correction in degrees
float pitchGyro = 0;                      //Pitch according to gyro 
float pitchAccel = 0;                     //Pitch according to accelerometers
float pitchPrediction = 0;                //Output of Kalman filter
float rollGyro = 0;                       //Roll according to gyro
float rollAccel = 0;                      //Roll according to accelerometers
float rollPrediction = 0;                 //Output of Kalman filter
float giroVar = 0.1;                      //Was 0.1
float deltaGiroVar = 0.1;                 //Was 0.1
float accelVar = 5;                       //Was 5
float Pxx = 0.1;                          //Angle variance; was 0.1
float Pvv = 0.1;                          //Angle change rate variance; was 0.1
float Pxv = 0.1;                          //Angle and angle change rate covariance; was 0.1
float kx, kv;                             //Mysterious Kalman thingies
const int FIRArrayLength = 36;            //Number of taps for low pass FIR filter
float accelXFIRArray[FIRArrayLength];     //Array for FIR calculations
float accelYFIRArray[FIRArrayLength];     //Array for FIR calculations
float accelZFIRArray[FIRArrayLength];     //Array for FIR calculations
float gyroXFIRArray[FIRArrayLength];      //Array for FIR calculations
float gyroYFIRArray[FIRArrayLength];      //Array for FIR calculations
float FIRFilter[FIRArrayLength] = {       //FIR filter coefficients for: 0-4Hz: gain 5, ripple 12dB; 10-50Hz: gain 0.000001, ripple 72dB; http://t-filter.appspot.com
  0.00008103385604967716,
  0.00027111799242077064,
  0.0006854974917591413,
  0.0014576515867607135,
  0.0027530631363864716,
  0.004753901748829375,
  0.007635997857285217,
  0.011541206789928664,
  0.016544708776664904,
  0.022626620444902264,
  0.02964895502860427,
  0.03734831257674501,
  0.045342366754130024,
  0.053157699234441635,
  0.060270435821884946,
  0.06616236694645264,
  0.0703764023237606,
  0.07257493449372505,
  0.07257493449372505,
  0.0703764023237606,
  0.06616236694645264,
  0.060270435821884946,
  0.053157699234441635,
  0.045342366754130024,
  0.03734831257674501,
  0.02964895502860427,
  0.022626620444902264,
  0.016544708776664904,
  0.011541206789928664,
  0.007635997857285217,
  0.004753901748829375,
  0.0027530631363864716,
  0.0014576515867607135,
  0.0006854974917591413,
  0.00027111799242077064,
  0.00008103385604967716
};
const int zeros = 10;
const int poles = 10;
float accelXInput[zeros + 1], accelXOutput[poles + 1];
float accelYInput[zeros + 1], accelYOutput[poles + 1];
float accelZInput[zeros + 1], accelZOutput[poles + 1];
float gyroXInput[zeros + 1], gyroXOutput[poles + 1];
float gyroYInput[zeros + 1], gyroYOutput[poles + 1];
float minServoAngle = 50;                 //Limits to the throw
float maxServoAngle = 130;                //of the servo arms
unsigned long timer;                      //Timer for calibrating loop time to timeStep seconds.
Servo leftServo, rightServo, throttleServo, rudderServo;
float pFactor = 1.5;                      //Multiplier between angle errors and servo signals
float leftCalc, rightCalc, leftServoSignal, rightServoSignal; 

//Function for writing a byte to an I2C device
void writeTo(byte device, byte toAddress, byte val) {
  Wire.beginTransmission(device);  
  Wire.send(toAddress);        
  Wire.send(val);        
  Wire.endTransmission();
}

//Function for reading num bytes from an I2C device
void readFrom(byte device, byte fromAddress, int num, byte result[]) {
  Wire.beginTransmission(device);
  Wire.send(fromAddress);
  Wire.endTransmission();
  Wire.requestFrom((int)device, num);
  int i = 0;
  while(Wire.available()) {
    result[i] = Wire.receive();
    i++;
  }
}

//Function for reading gyroscopes
void getGyroscopeReadings(int gyroResult[]) {
  byte buffer[6];
  readFrom(0x68,0x1D,6,buffer);
  gyroResult[0] = (((int)buffer[0]) << 8 ) | buffer[1];
  gyroResult[1] = (((int)buffer[2]) << 8 ) | buffer[3];
  gyroResult[2] = (((int)buffer[4]) << 8 ) | buffer[5];
} 

//Funcion for reading accelerometers
void getAccelerometerReadings(int accelResult[]) {
  byte buffer[6];
  readFrom(0x53,0x32,6,buffer);
  accelResult[0] = (((int)buffer[1]) << 8 ) | buffer[0];
  accelResult[1] = (((int)buffer[3]) << 8 ) | buffer[2];
  accelResult[2] = (((int)buffer[5]) << 8 ) | buffer[4];
}

//FIR filter function
float FIR(float value, float array[]) {
  int i;
  float FIROutput = 0;
  for(i = FIRArrayLength-1; i > 0; i--){
    array[i] = array[i-1];
    FIROutput += array[i] * FIRFilter[i];
  }
  array[0] = value;
  FIROutput += array[0] * FIRFilter[0];
  return FIROutput;
}

//IIR filter function (Low pass Bessel, 5Hz corner freq., 10th order, 100 Hz sample rate; http://www-users.cs.york.ac.uk/~fisher/mkfilter/trad.html)
float IIR(float value, float xv[], float yv[]) {
   xv[0] = xv[1]; xv[1] = xv[2]; xv[2] = xv[3]; xv[3] = xv[4]; xv[4] = xv[5]; xv[5] = xv[6]; xv[6] = xv[7]; xv[7] = xv[8]; xv[8] = xv[9]; xv[9] = xv[10]; 
   xv[10] = value / 5.681713320e+05;
   yv[0] = yv[1]; yv[1] = yv[2]; yv[2] = yv[3]; yv[3] = yv[4]; yv[4] = yv[5]; yv[5] = yv[6]; yv[6] = yv[7]; yv[7] = yv[8]; yv[8] = yv[9]; yv[9] = yv[10]; 
   yv[10] = (xv[0] + xv[10]) + 10 * (xv[1] + xv[9]) + 45 * (xv[2] + xv[8])
            + 120 * (xv[3] + xv[7]) + 210 * (xv[4] + xv[6]) + 252 * xv[5]
            + ( -0.0084477842 * yv[0]) + (  0.1250487070 * yv[1])
            + ( -0.8452516757 * yv[2]) + (  3.4397583115 * yv[3])
            + ( -9.3457158590 * yv[4]) + ( 17.7418428080 * yv[5])
            + (-23.8769055880 * yv[6]) + ( 22.5422166190 * yv[7])
            + (-14.3254341650 * yv[8]) + (  5.5510863543 * yv[9]);
   return yv[10];
}

void setup() {
  //Variables used for removing zero bias
  int totalGyroXValues = 0;
  int totalGyroYValues = 0;
  int totalGyroZValues = 0;
  int totalAccelXValues = 0;
  int totalAccelYValues = 0;
  int totalAccelZValues = 0;
  int i;
  
  //LED on when calibrating
  pinMode(13, OUTPUT);   
  digitalWrite(13, HIGH);
  
  //Hook up the servos to digital pins 2 thru 5
  leftServo.attach(5, 600, 2400);
  rightServo.attach(4, 600, 2400);
  throttleServo.attach(3, 600, 2400);
  rudderServo.attach(2, 600, 2400);
  
  //Configure pins for throttle potmeter
  pinMode(A0, OUTPUT);
  digitalWrite(A0, HIGH);
  pinMode(A1, INPUT);
  pinMode(A2, OUTPUT);
  digitalWrite(A2, LOW);
  
  //Set the servos
  leftServo.write(90);
  rightServo.write(90);
  throttleServo.write(0);
  rudderServo.write(90);
  delay(50);
  
  //Open I2C and serial communications
  Wire.begin(); 
  Serial.begin(115200);
  
  //Apply settings to sensors
  writeTo(0x53,0x31,0x09); //Set accelerometer to 11bit, +/-4g
  writeTo(0x53,0x2D,0x08); //Set accelerometer to measure mode
  writeTo(0x68,0x16,0x1E); //Set gyro to +/-2000deg/sec, 1kHz internal sampling rate and 5Hz low pass filter
  writeTo(0x68,0x15,0x0A); //Set gyro to 100Hz external sample rate
  
  //Collect 50 measurements and calculate bias for sensors
  delay(100);
  for (i = 0; i < 50; i += 1) {
    getGyroscopeReadings(gyroResult);
    getAccelerometerReadings(accelResult);
    totalGyroXValues += gyroResult[0];
    totalGyroYValues += gyroResult[1];
    totalGyroZValues += gyroResult[2];
    totalAccelXValues += accelResult[0];
    totalAccelYValues += accelResult[1];
    totalAccelZValues += accelResult[2];
    delay(50);
  }
  biasGyroX = totalGyroXValues / 50;
  biasGyroY = totalGyroYValues / 50;
  biasGyroZ = totalGyroZValues / 50;
  biasAccelX = totalAccelXValues / 50;
  biasAccelY = totalAccelYValues / 50;
  biasAccelZ = (totalAccelZValues / 50) - 256;
  
  //LED off when done calibrating
  digitalWrite(13, LOW);
}

void loop() {
  //Initialise the loop timer
  timer = millis();
  
  //Set the throttle to the value read from the potmeter
  throttleServo.write(map(analogRead(1), 0, 1023, 0, 179));
  
  //Get the raw sensor data
  getGyroscopeReadings(gyroResult);
  getAccelerometerReadings(accelResult);
  
  //Remove bias from raw sensor data and convert to g and deg/s respectively.
  accelX = (accelResult[0] - biasAccelX) / 256;
  accelY = (accelResult[1] - biasAccelY) / 256;
  accelZ = (accelResult[2] - biasAccelZ) / 256;
  gyroX = (gyroResult[0] - biasGyroX) / 14.375;
  gyroY = (gyroResult[1] - biasGyroY) / 14.375;
  
  //Try to remove the motor and rotor vibrations from the data with a low-pass FIR/IIR filter
  //accelX = FIR(accelX, accelXFIRArray);
  //accelY = FIR(accelY, accelYFIRArray);
  //accelZ = FIR(accelZ, accelZFIRArray);
  //gyroX = FIR(gyroX, gyroXFIRArray);
  //gyroY = FIR(gyroY, gyroYFIRArray);
  accelX = IIR(accelX, accelXInput, accelXOutput);
  accelY = IIR(accelY, accelYInput, accelYOutput);
  accelZ = IIR(accelZ, accelZInput, accelZOutput);
  gyroX = IIR(gyroX, gyroXInput, gyroXOutput);
  gyroY = IIR(gyroY, gyroYInput, gyroYOutput);
  
  //Calculate angles from the sensor data
  pitchAccel = atan2(accelY, accelZ) * 360.0 / (2*PI);
  pitchGyro = pitchGyro + (gyroX * timeStep);
  pitchPrediction = pitchPrediction + (gyroX * timeStep);
  rollAccel = atan2(accelX, accelZ) * 360.0 / (2*PI);
  rollGyro = rollGyro - gyroY * timeStep; 
  rollPrediction = rollPrediction - (gyroY * timeStep);

  //Do the Kalman thing
  Pxx += timeStep * (2 * Pxv + timeStep * Pvv);
  Pxv += timeStep * Pvv;
  Pxx += timeStep * giroVar;
  Pvv += timeStep * deltaGiroVar;
  kx = Pxx * (1 / (Pxx + accelVar));
  kv = Pxv * (1 / (Pxx + accelVar));
  pitchPrediction += (pitchAccel - pitchPrediction) * kx;
  rollPrediction += (rollAccel - rollPrediction) * kx;
  Pxx *= (1 - kx);
  Pxv *= (1 - kx);
  Pvv -= kv * Pxv;
  
  //Use the Kalman output in a controller, limiting the maximum servo arms throw
  leftCalc = pitchPrediction - rollPrediction;
  rightCalc = pitchPrediction + rollPrediction;
  leftServoSignal = 90.0 - (pFactor * leftCalc);
  rightServoSignal = 90.0 + (pFactor * rightCalc);
  if (leftServoSignal > maxServoAngle) {leftServoSignal = maxServoAngle;};
  if (leftServoSignal < minServoAngle) {leftServoSignal = minServoAngle;};
  if (rightServoSignal > maxServoAngle) {rightServoSignal = maxServoAngle;};
  if (rightServoSignal < minServoAngle) {rightServoSignal = minServoAngle;};
  
  //Update the servo signal every loopsPerServoUpdate loops, aiming for once every 20ms
  if (++loopCounter >= loopsPerServoUpdate) { 
    leftServo.write((int)round(leftServoSignal));
    rightServo.write((int)round(rightServoSignal));
    loopCounter = 0;
  }

  //Make the loop last timeStep seconds
  timer = millis() - timer;
  timer = (int)(round(timeStep * 1000)) - timer; 
  delay(timer);
}

To be continued ...

top


This page was created by Oscar den Uijl, oscar@den-uijl.nl