"Not another Blog post" you say, "Get off your *** and code something up I hear you say" :)
Yeh I know, I hope to have some time over xmas. I am using this BLOG to clarify what I want to do, attempt to make notes that I can refer to (when I finally get my finger out), and get feedback from people to better understand how to best achieve my goal.
Just a quick note on the diagram above. I describe it below but I have remove a number of components that we currently use in the code because they unnecessarily complicate the drawing and are not used in my setup. They could be included in the controller if the user desired.
My Goal is to improve the ACRO mode
The Arducopter has been developed with a significant focus on autonomous flight. The hardware and IMU code is working extremely well. However ACRO mode is much simpler than the quality of the majority of this code and hardware deserves.
The unrestricted ROLL_PITCH_ACRO code is found here:
However it is combined with a YAW_HOLD code by default, shown here:
instead of the YAW_ACRO code here:
I understand this has been because it significantly improves when flying a helicopter.
There is also some restricted ACRO code, that is accessed by setting AXIS_ENABLE to 1, here:
The difference between what I refer to as restricted and unrestricted is weather or not the code allows the airframe to rotate through 360 degrees in any direction.
True ACRO mode will allow the pilot to do flips in roll and yaw or any combination there of.
Restricted ACRO mode is basically controlling STABILIZE mode using a rate input rather than an absolute angle input.
Ideal ACRO mode
The idea ACRO mode is "Fly by wire". By that I mean that the airframe is stabilized as it is in STABILIZE mode but controlled by rate inputs like it is and ACRO mode. I have shown a number of PID control loops above that illustrate potential options to implement this.
The first is the rate only control loop that is currently used by default. This PID design suffers from the absence of absolute attitude feedback. This means that noise, sensor drift, and external forces can cause the attitude of the airframe to vary when no variation is commanded.
The second is the rate controlled stabilize loop that can be set by assigning AXIS_ENABLE to 1. This is a very nice option in that it allows small and precise attitude changes to be made by small stick inputs. This makes it possible to precisely control the airframe not only at hover but also in fast forward flight when pitched forward at 30 degrees. This mode could also be combined with the ALT_HOLD throttle mode to make learning to fly ACRO easier. However, this PID design doesn't result in crisp response because the rate is only increased in proportion to the difference between the desired attitude and measured attitude. This difference must build up to achieve full rate. The last option addresses this problem using a feed forward technique.
The third option is what I would like to eventually implement. This is a feed forward PID design. Here the desired rate is directly fed into the rate controller. However unlike the rate only PID design, the rate is also fed into the attitude input via an integrator (or rate times time step sum). The attitude feedback part of this PID controller only attempts to correct the error in attitude, it is not responsible for commanding the desired rate. In this way we are able to achieve full rate without the "wind up" effect of the attitude controller but still achieve a very stable attitude when zero rate is commanded. If the copter is hit by an external force, the attitude feedback part of the PID will reset the attitude to where it should be. This brings me to the attitude error limit. This is there to prevent poor PID coefficient choices from causing the airframe to continue to move far after the rate command has been set to zero.
Gimbal Lock and Ardupilot
This brings me to a very important point that is missing if we want to be able to use an unrestricted and stabilized implementation of ACRO. Currently the roll and pitch controllers assume that the commands to move the airframe in roll, pitch and yaw correspond to the measurements of attitude. While this is not far from the truth during hover it creates a larger and larger error as the pitch or roll angles increase.
This made flying the quad very strange for me initially. I am used to flying aircraft where I could "Bank and Yank" the aircraft through a turn (roll to say 45 degrees and use pitch to turn). Because of this effect we need to "Bank and Yaw" instead.
While rolled 45 degrees to the right, an input in Yaw to the right (measured by the attitude controller as an angle from north around the horizon) will cause an equal rotation rate in both Yaw to the right and Pitch down. This will cause the Pitch controller to add a pitch up once as this error grows but because this pitch up is at 45 degrees from the horizon it will cause both an increase in pitch up and an increase in yaw to the right.
Ideally what we would do is translate the coordinate system of the earth to that of the airframe before commanding a desired pitch, roll and yaw rate. In this case if the airframe is rolled 45 degrees to the right and a yaw command to the right is applied the yaw controller will command both a yaw and pitch rate command that keeps the nose of the airframe on the horizon.
Without this facility we can't implement a stabilized, unlimited ACRO mode.
Stability in ACRO mode
The common perception of perfect ACRO mode is that we command a rate in any axis and the airframe moves in that axis. While this is good, it is what is known as neutrally stable. When we fly an aircraft we like the aircraft to have a tendency to right its self without user input, this is known as positively stable. This can be implemented into the PID controller using a user defined attitude to rate feedback loop. Basically the roll and pitch angle is multiplied by a stability factor and subtracted from the desired roll and pitch rate. This results in the airframe moving back to level when the sticks are released in a similar way to STABILIZE but when full stick is applied the airframe will continue to rotate in what ever direction is commanded.
An ACRO beginner would set the stability factor high resulting in performance very close to STABILIZE while an experienced pilot might set it to zero or very small so that the attitude can be set not move if the sticks are let go.
OK it's over
Well this pretty much sums up my intentions. The reason I have chosen to focus on ACRO is because most of what I describe above can be done without effecting any other of the auto modes (limited stabilized ACRO mode with a stability factor). And this also happens to be what I want to fly with when I do my 55 km/h runs down a valley out the back of a friends farm house.(Video to come, but I want to get lower).
Any way, if you have read all the way to this point I congratulate your patience. Thank you in advance for any feedback, comments, and (especially) corrections, you can give on the ideas expressed above.
You may be happy to know that I think I am out of things I want to put on my BLOG now :) Now I just need to fix my USB port on my ARM2 so that I can program it without holding the USB cable.
Equations for translation between EARTH frame and BODY frame:
BODY_RATE_YAW = cos(PITCH_Angle) x cos(ROLL_ANGLE) x EARTH_RATE_YAW +
sin(ROLL_ANGLE) x EARTH_RATE_PITCH;
BODY_RATE_PITCH = cos(ROLL_ANGLE) x EARTH_RATE_PITCH +
sin(ROLL_ANGLE) x EARTH_RATE_YAW;
BODY_RATE_ROLL = EARTH_RATE_ROLL + sin(PITCH_ANGLE) x EARTH_RATE_YAW;