Phidgets Accelerometers, the Magic of Three-Axes

The three-axis accelerometer is a piezo-electric accelerometer that is about 1" square. It is pretty good and have never seen any issues with drift or orientation issues. It measures each axis in units of g Do not forget to convert to your units, so a reading of 1.2 is actually an acceleration of 38.6 ft/s/s. I just make a little helper function to convert the readings when the phidgets_accelerationchanged method fires.

Hopefully, that will help clean up your code by reducing the risk of double converting units. Even NASA makes this mistake. One of the first things that I set up is a library of unit converstion factors. That way it is less risky if the user wishes to see the measurements in , mks or cgs and my software interanlly uses US Customary slug-ft-lbf. I do not do any conversions in the code. Just read the electrical signals from the transducers and convert them to real units, consistent with the system. Do not try and convert back and forth within the code, it will just be miserable to find.

You convert to any system that is different than your base unit system until you display the data. You can easily set a flag in the display object that shows the data to the user and multiply out the measurements at presentation time via the decoration pattern. To be honest, the Phidgets API is awesome. It makes short work of connecting and managing their instruments, so the code to start an interface kit is not so different from the accelerometer.

do
{
//System.Threading.Thread.Sleep(10);
if (acc0.Attached)
{

Console.WriteLine("_acc0 attached");
//accelerometer events
acc0.Attach += phidgets_Attach;
acc0.Detach += phidgets_Detach;
acc0.Error += phidgets_Error;
acc0.AccelerationChange += _acc0_AccelerationChange;
}
else
{
Console.WriteLine("retry : " + retry + " waiting for acc0 attach");

}
retry++;
} while (retry < 10 && !acc0.Attached); #region helperMethods #region Phidgets event handlers ///

/// handle the phidget device discovery events
///

/// /// protected void phidgets_Attach(object Sender, AttachEventArgs Args)
{
try
{
Console.WriteLine(Args.Device.Type + " attached.");
}
catch (Exception _exc)
{
throw new Exception(className + " protected void phidgets_Attach( Sender, Args) :: " + _exc.Message +
"\n");
}
} //phidgets_Attach
///

/// handle the phidget device discovery events
///

/// /// protected void phidgets_Detach(object Sender, DetachEventArgs Args)
{
try
{
Console.WriteLine(Args.Device.Type + " detached.");
}
catch (Exception _exc)
{
throw new Exception(className + " protected void phidgets_Detach( Sender, Args) :: " + _exc.Message +"\n");
}
} //phidgets_Detach
protected void phidgets_Error(object Sender, ErrorEventArgs Args)
{
try
{
Console.WriteLine("phidgets error : " + Args.Code + " " + Args.Description);
}
catch (Exception _exc)
{
throw new Exception(className + " protected void phidgets_Error( Sender, Args) :: " + _exc.Message +
"\n");
}
} //phidgets_Error
///

/// reads the acceleration from the Phidgets accelerometer
///

/// accelerometer object/// essentially an array of three doubles, one for each direction measuredprotected void _acc0_AccelerationChange(object Sender, AccelerationChangeEventArgs Args)
{
try
{
rawAcc[Args.Index] = Args.Acceleration;
}
catch (Exception _exc)
{
Console.WriteLine(
className + " protected void _acc0_AccelerationChange( Sender, Args) :: " + _exc.Message + "\n"
);
}
} //_acc0_AccelerationChange
#endregion

Another helper method that is constantly requested is converting from accelerations to roll and pitch. You can do the trigonometry yourself, but if gravitation is assumed to act in the -Z direction you can work out the basic orientation of the accelerating object. This can be fooled by large or quick orientation changes, but for the most part sampling frequency can fix this. So I would make sure that you do as little as possible that may muddy the event handler system. They are really fast and that is a good thing in this case.

///

/// calculate the euler angles from the local accelerations
///

/// acceleration toward the right wing, g [gravity multiples]/// acceleration toward the nose, g [gravity multiples]/// acceleration toward the ground, g [gravity multiples]/// headingprivate static void accel2euler(double Ax, double Ay, double Az, double Compass, out double[] EulerAngles)
{
EulerAngles = new double[3];
try
{
double g = Math.Sqrt(Ax * Ax + Ay * Ay + Az * Az);
/* Roll */
if (g != 0)
{
//EulerAngles[0]=Math.Atan2(Ay,Az);
EulerAngles[0] = Math.Atan2(Ay/g, -Az/g);
}else
{
EulerAngles[0] = Math.Atan2(Ay / 1, -Az / 1);
}
/* Pitch */
if (g != 0)
{
//EulerAngles[1] = Math.Asin(Ax/-g);
EulerAngles[1] = Math.Atan2(Ax / g, -Az / g);
}
else
{
EulerAngles[1] = Math.Atan2(Ax / 1, -Az / 1);
}
EulerAngles[2] = Compass; /* Yaw */


}
catch (Exception _exc)
{
throw new Exception(className + " public static void accel2euler( , " + Ax.ToString("0.000") + " , " +
Ay.ToString("0.000") + " , " + Az.ToString("0.000") + " , " +
Compass.ToString("0.000") + " ) :: " + _exc.Message + "\n");
}
}

The one thing you will notice is that you cannot get the yaw from the accelerations. That makes sense if you think about it, flat rotation perpendicular to gravity would not be measured. I usually run a compass in the systems too. That makes the 3-1-3 rotation easy to move between body reference frames and global reference frames. I would suggest that you multiply out the cells for the rotations in a separate method each so that you can just multiply them by calling each method in turn with an argument of the last rotation.

Before we get to actual code...

Before we hop off into any actual implementation, I would suggest that we sit and think about some basics. The very first thing that should be done is to come up with a basic skeleton of the code that you would like to write. This may impact many decisions down the line. Basically, a UGV and a UAS/UAV are the same from the code's perspective. The UAV/UAS use different actual algorithms for certain processes, but the data flow and data collected are not so different. Some data is read, a decision is made, a servo solution is calculated, servos are moved into position and then the system loops again.

One thing that I did, was to develop an object that was a sensor, ISensor. This will be the part that actually interact with any device. ISensorReading will also act as a fundamental contract for data. I would make sure that this object follows in general an observable pattern. That way it is easy to allow other objects to read its data, or to be notified when new data is available. This will make it easy for filtering algorithms to execute and work on the newest data as soon as it arrives. It will also make it easier to make the data collection to be asynchronously collected and processed.

Then I made an instrumentation manager, IM. The IM will have factories for each kind of sensor and observe instances of each of the instruments. This way if something happens the IM can close an instrument and try to reconnect with a new object. In turn, the IM presents data to other parts of the code as needed. No other parts of the code need to see the sensors or interact with them directly.

On top of the instrumentation system, should be a director layer. This will actually be the layer that does all of the work. It makes a nice break line between the device and the presentation, or intelligence. Separating the systems is an important object-oriented technique that is necessary for keeping the code healthy.

More about the director later... I will see if I can get some pictures in to make it easier to see.

Basic Quantities and Some Trigonometry

So You Want to Build a DIY Autopilot

Accelerations

One of the most important quantities for you to measure are accelerations. If your two or more-axis accelerometer is mounted along the traditional axes of the aircraft it will be the easiest to code for. The three traditional axes are:

• From the nose through the center of gravity on the line of symmetry, the Y-axis, roll
• From the center of gravity out of the fuselage toward the tip of the right wing, the X-axis, pitch
• From the center of gravity away from the earth, the Z-axis, yaw
  
     

Figure of Rigid Aircraft Axes

So, now for some basic Trigonometry, everyone remembers SOH CAH TOA.

In general, the following picture is true for a vehicle moving through space.

Figure of the Direction of the Force of Gravitation on a Body

As you can see from the image, the angle of pitch relative to the surface of the earth is the same angle offset of the weight vector relative to the z-axis in the body frame of reference. If we put our accelerometer so that one of its axes is parallel to the body's z-axis at its center of gravity we are measuring this offset vector. Which is really neat, because it means that we can express the angle of the body in level, non-accelerating motion as ratios of the accelerations

Pitch: 

The angle theta between the actual gravity vector and the measured gravity is related to the pitch of the aircraft (pitch = theta + 90°). If we know theta, we know our pitch! Since we know the magnitude of the earth’s gravity, simple calculus gives us our pitch angle:

accelerometer = cos (theta) * gravity

theta = acos (accelerometer / gravity)

And since pitch = theta + 90°

pitch = asin (accelerometer / gravity)

Woot, we calculated the pitch orientation of our airplane using an accelerometer. Pretty easy, huh?

The real formula that we need to use for the software looks like this:

pitch = atan2(accelerometer / gravity, z / gravity)

Common piezo-electric accelerometers return in units of, g, 32.17 ft/s^2 or 9.81 m/s^2. We also know some more things about the flight that let us calculate the angles relative to the ground. More on this later, it is a bit more than basic trigonometry to describe. These equations assume non-accelerating flight. You can use a magnetometer to get the relative plane in space with less math, but magnetometers generally take more interface programming in my experience.

Roll:

Roll needs a second accelerometer with an axis perpendicular to the first so that we can figure out the resultant vector between them and then the angle. Essentially the vector between the accelerometers becomes the "gravitational" acceleration and the relative readings lets us calculate the angle with an atan2 function. The second accelerometer will have some other things to manage such as the effects of the distance between them on the accelerations measured. Physics fun and none of the boring class.

Yaw:

Yaw is the hardest of the angles to measure. The only answer is to use a magnetometer or a compass. In many ways, yaw can be solved by dead reckoning. Dead reckoning is all that is important for most of the projects in the DIY garage. They will be covered later.

Next we will discuss gyroscopes and the beauty of rates and integral calculus

How I started writing a DIY Autopilot

My intention still is to write my own c# based autopilot. It is not trivial as I found out. Lots and lots of details are needed. The first thing that needs to be done is to determine the instruments that you will need to measure the quantities that you need to know to control flight. Even this seems to be a matter of opinion. For a reference, I found every group that I could that was writing their own version.

From my perspective the quantities that we need to know to control flight are:

• air speed, ft/s or m/s
• altitude, ft or m
• orientation, roll, but pitch is good

These are intrinsic quantities that are really easy to do yourself. Air speed is the model's forward velocity. Altitude is the vehicle's position relative to the surface of the earth. Pitch and roll are the angles of the vehicle relative to the ground. Let me rephrase that, roll is the angle between the right wing and a plane parallel to the ground. Pitch is also known as angle of attack. Pitch is the angle between the plane at the center of the vehicle located on vectors from the center of gravity to the right wing tip and the center of gravity to the tip of the nose and the relative wind.

At the beginning you will need to measure the following quantities:

• air pressure
• acceleration in at least two directions
  

Pressure taps are the easiest ways to measure air pressure in a flying vehicle. Piezo-electric accelerometers are cheap these days and are really accurate. In the next post we will talk about issues with these instruments and the physical quantities that you are actually measuring and how to use those as a basis for a control system.

For all of those inch haters, a quick note about units. You can do this in any unit system. It is unimportant. I will write a quick post on the conversions between the US Customary System and the metric (mks) system. Remember metric is actually several unit systems in one and you have to keep them consistent. Oh, and for those trying to buy nuts and bolts, Japanese manufacturers use odd metric sizes, European use even metric sizes, and the US Customary System/SAE is in units of 1/64th of an inch. So SAE/SAME will have three bolt sizes for every bolt size in the two metric systems. Remember that the thread counts are different though.

In my opinion, if the units are managed consistently then the issue is one of presentation to the user. Part of the display system of my code will show you how to do this with a simple object oriented approach.