Hi, I have taken a quick look at the code. Now let's see here.... I have understood that the main feautures of this code version (gentleNAV) implements:
A state machine
Logic is interrupt controlled
Has a ticker bacground task for low-pri "user tasks"
Uses hw funcionality of the DsPIC chip implemented with macros
Specifically the multiply functions are hw mapped
Uses a CORDIC rotational algoritm do to the rectangular to polar coordinate operations
It does assume that the plane is stable so no aileron controls / loops are needed or implemented?
You have two loops (PI loops):
(__builtin_mulss((zrate.filtered-zrate.offset+vref.filtered-vref.offset), YAWGAIN )>>8)
+ __builtin_mulss( ((int)error_dir ), STEERINGGAIN ) ;
This uses the Z gyro filtered and the steering error and two GAIN constants
((__builtin_mulss(( xrate.filtered - xrate.offset + vref.filtered -vref.offset ), PITCHGAIN )
+ __builtin_mulss( (yaccel.filtered - yaccel.offset) , ACCELGAIN ))>>8) ;
This uses X gyro (nick/pitch rate = angular velocity of nose dip) and Y accelerometer (drop rate in (m/s^2) normalized) and adds those two and uses the GAIN constants in addition.
Have I understood the main outline of the code correctly?
You understand the main outline of the code correctly. I am impressed with how quickly you were able to do that.
The code evolved during my experiments with my previous board using its 2 gyros and one of its 2 accelerometers, and a GPS radio. It was flown on a Gentle Lady sailplane, so that there was a great deal of stability to begin with, and so that I did not have to worry much about the bank angle. The idea was to steer the plane from the GPS. I started out with several terms in two control loops for rudder and elevator, including rudder-elevator mixing and pitch-yaw gyro decoupling and gradually moved to the rather simple loops that you refer to. The performance of the control loops has been satisfactory, I routinely use the controller while flying my sailplane. Its predictable, smooth, stable, and well behaved. I switch on the pitch stabilizer to eliminate "porpoising", and I switch on the rudder control loop to bring the plane back to me.
Both loops use proportional plus derivative feedback, plus a small amount of filtering to reduce noise. You referred to a PI controller, I am not exactly sure what you meant, I take PI to mean proportional plus integral. If you take the point of view that we are trying to control pitch and yaw rate, then you can say that the control loop is PI control. In general you might want to use a PID controller, but I did not need to for what I wanted to do, a PD control was fine.
The rudder loop controls yaw angle. Proportional direction feedback comes from the GPS yaw information. Derivative feedback is from the yaw gyro, which measures the time derivative of the yaw angle. The derivative term stabilizes the loop, and enables moderate values of GPS feedback without going unstable in the face of 1/second GPS reporting and the inherent group delay of the filters used internally by the GPS.
Similarly for the elevator loop. The accelerometer that I use is the one that measures acceleration along the fuselage. The accelerometer furnishes proportional pitch feedback, the pitch gyro furnishes pitch derivative feedback. The derivative term stabilizes the loop in the face of the delay in the response of the accelerometer to changes in pitch angle while the speed of the plane adjusts to the new pitch angle. However, because I do not do any mixing or decoupling to account for bank angle, turns have to be gentle or the plane will tend to "swoop" around the turn.
Regarding macros, there is actually only one macro in my code that I wrote myself. At the beginning of each interrupt routine I call a macro that I use to estimate processor loading by sending a signal out on one of the spare pins. All of the other constructs that look like macro calls are either for macros that come with the Microchip C compiler, or which refer to C structures and unions that I have defined. The use of the macro makes it easy for me to turn the diagnostic on or off.
The Microchip macros that I use start with "__" and are used either for direct access of some of the CPU features, or to set up the properties of interrupt routines. The support provided by the compiler for the CPU is very well thought out.
I found the macro that multiplies two 16 bit numbers and returns a 32 bit result, using a single CPU instruction, to be particularly useful. Otherwise, the standard behavior of the code generated by a C compiler for the multiplication of two 16 bit numbers, will produce a 16 bit result. If you try to force the issue with a cast of the result to 32 bits, a compiler will do a lot of sign extending, shifting around, etc., and in the end you either wind up with a sign extended 16 bit result, or a lot of extra steps.
There are a few places in the code where you will see something like "._." or ".__._." in an expression. This is the way I was able to efficiently access 8 or 16 bit chunks of a 32 bit variable. It was particularly handy in the binary GPS parser. What I did was to use a combination of structure and union definitions in such a way that I could directly access all or portions of a variable without two much trouble, without causing the compiler to generate a lot of unnecessary instructions.
It is possible to do what you suggest, but not without modifying the "GentleNav" firmware, which is only a simple 2 axis sailplane controller intended as an example to get you started. To do what you have in mind you will need to extend the firmware. You can reuse much of the GentleNav firmware, but you will also have to write a fair amount of new code as well.
The gyros, accelerometers, and GPS provide XYZ location, rotation rate, acceleration, and velocity information, which, together with the assumption that the plane flies in the direction it is pointed, and including compensation for centrifugal effects, can be used to determine location and orientation of a plane rather accurately, as long as it doesn't fly purely vertically for long periods of time. The calculations to do this are not that difficult, I am working on them as we speak. I intend to have the firmware finished and published here by spring.
By the way, the GPS provides a rather accurate estimate of XYZ velocity, which is computed from Doppler shift. Granted it is reported only once per second, but it can be integrated with gyro and accelerometer information to provide a continuous, accurate estimate of velocity. Furthermore, the XYZ velocity estimator could be used to enhance the accuracy and transient response of the location estimator.
As far as i know most modern EFIS suites use gyros and acceleromenters to calculate pitch roll and heading (along with vertical and horizontal speed) but stabilize what they get using gps and magnetometer information. is this exactly what you are working on now?
Yes, I am going to work on an accurate yaw, pitch, roll estimator after I finish the documentation for the GentleNav firmware.
Here is my plan:
1. I am going to maintain accurate estimates of two vectors, the gravity vector with respect to the frame of reference of the plane, and the velocity vector of the plane with respect to the ground. I am going to approximate their rapid changes by integrating their cross products with the 3D rotation rate vector from the gyros. I will correct for drift in gyros by adjusting with low frequency information from the accelerometers and GPS, either with Kalman filters or with "washout" filters.
2. For the velocity vector I will adjust for drift using 3D velocity information from the GPS.
3. For the gravity vector I will adjust for drift using the 3 axis accelerometer. For this adjustment I will need to compensate the accelerometers for rotational acceleration, which I can do with the cross product of the velocity vector and the rotational vector, computed in the reference frame of the plane.
I do not plan to use a magnetometer.
Unlike modern EFIS suites, I plan to get vertical and horizontal speed mainly from a combination of GPS and gyros. I will be using accelerometers only indirectly to compensate for gyro drift. It is not commonly known that the GPS gives a rather accurate estimate of 3D velocity from Doppler shift, which is done using the baseband signals from the satellites, not the modulation. I have tested this out by simply moving my GPS around by hand, and find it to be amazingly sensitive and accurate. It is much more accurate than what you could compute by taking the difference in position divided by time.
The only "blind spot" is when the plane flies vertically for a long time. I do not plan to do this, mostly the plane will be flying in the horizontal plane.
4. As a further bonus, I may try to integrate the velocity vector (which I think will be rather accurate) to get position, and then use the GPS position to adjust for drift.
I do not have a timeline, this is my hobby. I don't have any goals in mind, for me its the journey, so if I get distracted by any of the many other interests I have, this could wind up on the back burner for a while. As Calvin and Hobbes once said, "The days are just packed."
Having said that, most likely I will have this flying by the end of fall 2009.
Thank you very much for this comment, and for all of your previous comments.
You are correct, in the case of mild to strong winds there will be a difference between the direction the plane is pointed and the direction it is moving, but I view this as a feature, rather than as a deviation. As I am sure that you know, the difference between the two angles turns out to be the "crab angle" the plane needs to keep it moving along the desired course over ground.
I have already seen this effect with my GentleNav firmware, which uses gyro and GPS to estimate yaw angle. When my sailplane returns to me in a mild to strong wind, it crabs by just the amount of angle it needs to compensate for the wind.
There is a secondary effect. There also is a variation in the effective feedback gain for turning. The effective gain goes up when flying into the wind, and it goes down when flying downwind, so it does cut into the stability margin when flying upwind. However, my stability margin is large enough so that I have not had a problem with winds, unless the wind is greater than the speed of my plane, but then I would not be flying in any case.
The net result is that if you treat the deviation as the required crab angle the plane flies where you want it to go, and the control is stable.
If you prefer to treat the effect explicitly, of course you would then have to estimate the wind speed. This could be done, for example, by flying in a circle.
I thought of another way of looking at the effects of winds.
The algorithm that I described can be used to control course over ground without knowing or caring what direction the plane is actually pointed or what the wind is. It will produce a continuous, accurate estimate of the plane's velocity vector, with respect to the ground. Any feedback control for steering the plane that uses the vector will transparently correct for wind without ever knowing what it is.
However, as I am sure that you will point out, without knowing the wind vector, it is not possible to compute true air speed from ground speed. However, so far, I have not used air speed in any of my control algorithms, and I think I can continue to get by without knowing air speed by using a combination of ground speed, pitch angle, and vertical speed as inputs to any speed control that I might want to use.
I would very much appreciate your thoughts on the subject.
Just out of curiousity, I do not quite follow the choice of the external 16MHz clock. In essence this means if you are using the HS setting in FOSC your CPU will be clocked with 4MHz (FCY == FOSC / 4). So your only real choice to get decent CPU performance is to use FRC w/PLL x16. In which case of course you'd loose the precision of having an external crystal ...