The PhidgetsException #9

For the most part, I really like Phidgets. They are good simple equipment that is inexpensive and just works. That is a trick in my experience in the current robotics technology playing field. Their API is the best I have seen and I have to say does a nice job even if you want to write in c# or flash. Yeah, Flash is supported.

So I was happily banging away at my control code to manage these servos. I kept seeing that the code was running. I sent lots and lots of new servo locations to the servos and it just did not move the servos. After some investigation into my logs and trapping lots of PhidgetExceptions. I figured out what the issue was. Like a good little OOP'er I had contained my reading of the Advanced Servo Controller 8 port's properties in an object and then a different handles the executions. Here in lay the problem, the properties of a given servo such as velocity which has no setter (mistake in the documentation) are not exposed until the motor is in engaged.

So I wrote the following code to fix that. These are not threadsafe, you have to make sure that you are not engaged at the wrong time. It will make all kinds of wacky stuff happen.

Viewing code:

public double GetServoPosition(int servoIndex)
{
double position=-1;
try
{
AdvancedServoServo ass=null;
if(advServo.Attached==true)
{
ass=advServo.Servos[servoIndex];
ass.Engaged=true;
position=ass.Position;
ass.Engaged=false;
}
}catch(Exception exc)
{
throw new Exception(className+" public double GetServoPosition( "+ servoIndex+" ) :: "+exc.Message);
}
return position;
}

Motion code:

///

/// move a servo to a new position
///

/// index of the servo to move /// new position, should be 0-100 /// bool if the servo is now in the correct position
public bool MoveServo(int ServoIndex, int newpos)
{
bool movedOk = false;
try
{
AdvancedServoServo curr = null;

double pos = 0.01 * newpos * (basicServoMax - basicServoMin) + basicServoMin;

if (pos > basicServoMax){pos = basicServoMax - 1;}
if (pos < basicServoMin){pos = basicServoMin + 1;} if (aServo0.Attached && ServoIndex<4) { curr = aServo0.servos[ServoIndex]; } if (aServo1.Attached && ServoIndex > 3)
{
curr = aServo0.servos[ServoIndex];
}

if (curr != null)
{
curr.Engaged = true;
if (Math.Abs((curr.Position - pos) / curr.Position) > 0.03)
{

curr.Position = pos;

//disengage the servo when we are not moving it
curr.Engaged = false;
}
}
}
catch (Exception exc)
{
if (LogEvent != null)
{
LogEvent(className + " public bool MoveServo( " + ServoIndex + " , " + newpos + ") :: " +
exc.Message + "\n", false);
}
else
{
throw new Exception(className + " public bool MoveServo( " + ServoIndex + " , " + newpos +
") :: " +
exc.Message + "\n");
}
}
return movedOk;
} //MoveServo

Servo reset code:

///

/// try to reset the servo, when it throws phidgets exceptions, set it to a safe midpoint
///

/// servo to reset void resetServo(AdvancedServoServo srvo)
{
try
{
srvo.Engaged = true;
double midpoint = (basicServoMax - basicServoMin) / 2 + basicServoMin;
srvo.Acceleration = 1000;
srvo.Position =midpoint;
srvo.Engaged = false;

}catch(Exception exc)
{
LogEvent(className + " void resetServo(AdvancedServoServo) :: "+exc.Message, false);
}
}

Another thing, that you have to make sure that you consider is that there are hard to document dead zones in each servo. They are sometimes inherent to the servo design, but each servo can have multiple in different places. I would make sure that if you hear this tell-tale clicking sound when the motor tries to move to the right place and then corrects ad nauseum... that you trap this in your code. My experience is that this burns up your motor pretty quickly. These are toy or better than toy motors. Better motors are available but are more expensive.

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.

Of Directors and Other School Marms

In my model, there is one king pin class. It does all of the general start to stop management of the system. The director class is where all of the managers hang from. I do not really hang a lot of factories off of the director, the  decorations to the class are minimal.

At start up, the director sets all of its properties from the configuration files and starts the managers. The managers in my model are:

• instrumentation manager, im
• task manager, tm
• ai manager, aim
• stability manager, stab man
• communication manager, comm
• display manager, display

This can be relatively complicated since the observation system must be handled by the director. The data flows from the instrumentation manager to the stability manager (stab man observes im). Task managers observe the communication manager to queue jobs, they also watch the stability manager. The Instrumentation Manager also watches the task manager, but only because it also contains the servo manager. Tasks go from the stability manager to the task stack and are then sent to the servo manager.

I thought this was the best way because whether or not the communications manager has a command, the task queue will be populated. The servo manager will then grab tasks and handle them in a first come first served pattern. If a command expires by sitting in the queue too long it is dumped by the task manager when the servo manager pops it off of the queue. As the system becomes more complicated, there could be more queues to do more things. However, tasks that require moving a servo always go to the servo queue.

I have read the great MAV blog by Tom Pycke. He has lots of things to say on the topic of real time operations. However, my idea is that they are not really so important. I do agree with him that his way of dealing with garbled or miscommunicated commands to the MAV is pretty interesting. My way seems to be not so bad either. The system must be able to fly itself, you tell it a basic plan to fly along. Any command inputs from the ground station are "exceptions".

Exceptions are easier to manage because you do them as soon as possible. When they expire you stop doing them, the system still tries to make the best of the situation and go back to the plan. This guidance could make a complete mess that breaks everything, in the case of coordinates that are for a different city. Your system may start flying off in a crazy direction trying to get home. However, that is why there are a few directives that the director knows and is the only one that can execute.

I keep a few tasks in the clip for a special occasion. My "director-only" tasks are:

• shutdown
• go to safe altitude
• land immediately
• idle

Shutdown is a good example of a game ender. If you shut down the computer systems and the current. For a UGS it may not be so bad, a UAS just falls out of the sky. In my overly instrumented systems, we have an aircraft on ground sensor. It is nothing as cool as a weight on wheels, but it does check to see how far the fuselage is off of the ground. This is to enforce a flight floor system, only in certain situations will it not try to climb to the safe altitude. In general, the safe altitude should be above the tree line. Altitude is safe for a UAS, this is not as necessary for a UGS but it can be interesting to have. Land immediately works for either kind of system, it forces the system to plot the fastest route to the start area/landing strip. Once the system arrives in the pattern, it will begin normal landing/parking processes. Idle is similar to shutdown, but it does not cut the power. It however, means RC override in implementation. The computer lets the system freewheel, this can be useful if the computer keeps doing crazy crap and you have to bring it back.

The ai (artificial intelligence) manager is the basis for the flight state. It reads the preset," at least we have somewhere to go" map and determines which mission phase the system should be in. Mission state is important information because it will time the use of instrumentation or cameras, as well as when to hit the "panic" button. The stability manager uses this to set system configurations and movement regimes for the  system.

A director class is a operator, directing the data from one manager to the other. The AI manager is where the state manager exists.  More on the AIM later, check in tomorrow if I am feeling feisty and have not cut myself up or glued something good to the table.

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.