Gimbal lock

== thread 1 ==
In this case, the AP did not initially lose orientation. The AP correctly banked the plane to turn it around. At that point, the AP appears to be working perfectly with the orientation shown correctly. The orientation as described in GE, matches that of the video, while the plane rolls over and begins a vertical descent. During the vertical descent (after the plane rolled over on it's back), there was some fast roll movements, as seen on the video that you enclosed (Thank you for the video). It is possible that the gyros saturated, I don't know the saturation max levels for the MPU6000 as used in our code, off hand. We can look at that later. Suffice to say, a saturated gyro would make the AP lose orientation. It will recover DCM yaw orientation over around 17 seconds of flight over the ground (movement required) once rotation rates return to being less than saturation levels.
It does appear that the autopilot did lose orientation during the descent, when fast rolls were occuring. Or to be more specific, after recovery from descent, to level flight, the Autopilot took a number of seconds to recover the correct DCM yaw orientation.
Best wishes, Pete
https://groups.google.com/forum/m/#!topic/uavdevboard/CyR9HBiZB5M
Gyro saturation
Andrius,
In addition to the effects mentioned by the team that would come into play during the dives that you saw, such as gyro saturation, and GE display problems for certain orientations, there is an effect that would certainly cause your flying wing to go into a near vertical dive if it got onto its back for some other reason, such as software/hardware bug. To see why, look at the following code in pitchCntrl.c:
if ( !canStabilizeInverted() || current_orientation != F_INVERTED )
{
rmat6 = rmat[6] ;
rmat7 = rmat[7] ;
rmat8 = rmat[8] ;
}
else
{
rmat6 = -rmat[6] ;
rmat7 = -rmat[7] ;
rmat8 = -rmat[8] ;
pitchAltitudeAdjust = -pitchAltitudeAdjust - INVNPITCH ;
}
In order for pitch control to be stable when the aircraft is on its back, as you can see, the signs must be flipped on rmat6-8 as well as pitchAltitudeAdjust. Otherwise, if the aircraft gets onto its back, the sign of pitch control feedback will go from negative to positive. Basically, that is because when the aircraft is on its back, the "elevator" deflection must be in the opposite direction relative to the aircraft.
There is a double "whammy". Once the plane its on its back, pointing down, both the altitude controls and pitch controls will work to point the plane down.
The net result is that the controls will find a new "stable" orientation, pointing straight down!!! (Yikes!!)
So, one thing you could do to prevent the vertical dives, especially in a flying wing, is to enable inverted stabilization.
I think Ben can tell you how to set it up.
=== Robert Mahoney ===
The ultimate solution to this problem is for me to implement the full version of Robert Mahoney's proposed controls. Basically, you: 
1 Multiply the actual DCM with the desired DCM to produce an error DCM. 
2 Convert error DCM to a desired correction rotation vector in earth frame. 
3 Transform rotation correction in earth frame to body frame. 
4 Map body frame desired rotation corrections onto flight surface deflections. 
This has been on my list of things to do for a long time. Its not so much needed for planes that tend to fly right side up, but for planes such as flying wings, I can see how the plane can easily get into a dive.That is not to say that this effect is the cause of the problems that you saw, only that it is a contributing factor.For some reason, your plane got flipped onto its back. After that, unless you have inverted flight enabled, the altitude controls will give you full up elevator and drive you plane into the ground. 
At the moment, I do not have time to dig into the telemetry and your options to see what is going on. I did watch the video. From the video, I offer the following thoughts and speculations to the other developers who might wish to help you: 
1 There is some risk of a spiral dive with a flying wing and a single waypoint. That is because the plane can actually get to the waypoint and stay there!! Then there are a number of factors that would cause trouble. For a flying wing, it is always better to define the RTL course to be a circle large enough that the aircraft can fly it with a bank angle of less than 45 degrees. 
2 I note that the controls gradually put the aircraft into a vertical spiral. So I suspect a combination of control effects led to the dive. 
3 Without a magnetometer, the IMU will completely lose track of roll attitude during vertical (up or down) flight. That is because without magnetometer, there is no roll reference (in the frame of aircraft) when the plane is vertical. Then, because of small errors in the gyro calibration, after a few turns, the roll orientation will be completely wrong, the controls will then have no hope of pulling out level, because as they start to do that, "up" and "down" might be mixed up, so pitch response will be wrong. 
4 In a sustained tight turn the GPS could very well stop supplying correct speed over ground and course over ground. In some of Ric's flights when he pushed the envelope in that fashion, the GPS gave up all together on course over ground. 
5 The default values for roll-elevator mixing in the repository are way too low. I typica 
=== thread 1a ===
Andrius,
Here is an addition to my last email.
In order to stabilize a plane on its back, the way that the MP is presently implemented, the signs of all feedback from earth frame must be flipped, for roll, pitch, and yaw control. (The full Mahoney approach would solve this problem.) So, when a plane gets on its back, roll, pitch, and yaw control will go unstable. Pitch/altitude/speed control will put the plane into a dive. (You see that speed itself is a problem, the speed control will pull up (up in the body frame) elevator to try to slow the plane down. The faster the plane dives, the worse it gets.) Yaw control will cause the plane to turn away from the RTL point, so the plane will fly in the opposite direction from what you want. And the roll control will attempt to roll the plane 360 degrees. The net result should be a spiral dive.
Here is the relevant yaw control code:
if ( ROLL_STABILIZATION_RUDDER && flags._.pitch_feedback )
{
if ( !desired_behavior._.inverted && !desired_behavior._.hover ) // normal
{
rollStabilization.WW = __builtin_mulsu( rmat[6] , rollkprud ) ;
}
else if ( desired_behavior._.inverted ) // inverted
{
rollStabilization.WW = - __builtin_mulsu( rmat[6] , rollkprud ) ;
}
rollStabilization.WW -= __builtin_mulus( rollkdrud , omegaAccum[1] ) ;
}
As you can see, aileron steering must be flipped when the plane is on its back.
Best regards,
Bill
=== control algorithm ===
The current control algorithm is aggressive in starting it's turns. Could this be causing the spins ?
For a 180 degree turn, the algorithm deflects the aileron completely and instantly to the maximum available for a given YAWKP. At slow speeds, could this possibly kick your flying wing into a spin ? As soon as the plane starts to bank, a number of algorithms start to reduce the aileron / elevons to based on the KD, rate based [Derivative based] constants, (as opposed to KP Proportional based constants). Those could be YAWKD (plane is yawing), or ROLLKD (rate of plane rolling into the bank). But whatever they do subsequently, initially the YAWKP constant is going to give you it's maximum yaw deflection based on YAWKP initially.
In a ground test, I would investigate the amount of initial aileron / elevon deflection for a 180 turn as follows:-
Put the plane in Auto mode, Move away from your waypoint 0 location (origin ?), buy about 50 meters or so (to be sure GPS is going to be sufficiently accurate to get the bearing for waypoint 0), put the plane in waypoint auto mode, so that the plane is trying to turn to the W0 location. Face the plane away from Home, and keep it horizontal.
 
Examine the aileron deflection.
That is your initial deflection that the plane will have on turning around. Would that deflection spin the plane at low speeds ? If you let the plane bank in the direction of aileron deflection, you will see them neutralise in a banking turn. At this point, the KD terms would normally be working, and the static test is no longer truly reflective of how the plane will work in the air ...... unless, like me on my trainer aircraft, you fly with most KD terms at zero. (which is probably not right for a flying wing).
=== Hilsim ===
Tom,
The HILSIM driver converts sensor data from floating point to 16 bit signed integer before transmitting to the UDB, so it should be clipping the gyro signals the same way the A/D would clip. The only thing is, the HILSIM scaling of the data transfer from Xplane to UDB is based on UDB2, so clipping might not match exactly if you are using UDB3,4,or 5, but there should be clipping in any case, and the gyro scale factors have not been all that different for UDB2, 3, 4 and 5. The only one that was much different was "UDB1".
Here is the relevant code:
 // Angular rate
 // multiply by 5632 (constant from UDB code)
 // Divide by SCALEGYRO(3.0 for red board)
 // 1 * 5632 / 3.0 = 1877.33
 Temp2.BB = (int)(P_plane * 1877.33);
 Store2LE(&NAV_BODYRATES[6], Temp2);
 Temp2.BB = (int)(Q_plane * 1877.33);
 Store2LE(&NAV_BODYRATES[8], Temp2);
 Temp2.BB = (int)(R_plane * 1877.33);
 Store2LE(&NAV_BODYRATES[10], Temp2);
Best regards,
Bill
=== Magnetometer ===
Andrius,
At the moment, I do not have time to dig into the telemetry and your options to see what is going on. I did watch the video. From the video, I offer the following thoughts and speculations to the other developers who might wish to help you:
1. There is some risk of a spiral dive with a flying wing and a single waypoint. That is because the plane can actually get to the waypoint and stay there!! Then there are a number of factors that would cause trouble. For a flying wing, it is always better to define the RTL course to be a circle large enough that the aircraft can fly it with a bank angle of less than 45 degrees.
2. I note that the controls gradually put the aircraft into a vertical spiral. So I suspect a combination of control effects led to the dive.
3. Without a magnetometer, the IMU will completely lose track of roll attitude during vertical (up or down) flight. That is because without magnetometer, there is no roll reference (in the frame of aircraft) when the plane is vertical. Then, because of small errors in the gyro calibration, after a few turns, the roll orientation will be completely wrong, the controls will then have no hope of pulling out level, because as they start to do that, "up" and "down" might be mixed up, so pitch response will be wrong.
4. In a sustained tight turn the GPS could very well stop supplying correct speed over ground and course over ground. In some of Ric's flights when he pushed the envelope in that fashion, the GPS gave up all together on course over ground.
5. The default values for roll-elevator mixing in the repository are way too low. I typically use 2.0 or 3.0 for mixing for my flights. (Older versions of MP max out at 1.9999). Without enough roll-elevator mixing, the controls will tend to put a flying wing into a vertical spiral. Not sure what impact rudder-elevator mixing would have on a flying wing. I would hope that the code ignores is for a flying wing, but it would be best to set it to zero.
6. It is important that the roll leveling feedback gain (ROLLKP) will provide more feedback than steering control (YAWKP_AILERON). Typically, ROLLKP should be at least as large as 2 times YAWKP_AILERON. The reason why that is critical is that if ROLLKP is not big enough, the aircraft will roll more than 90 degrees in an attempt to turn. Once it goes past 90 degrees in an attempt to turn, the roll control loop will fail, the ailerons will lock up, and the aircraft will do exactly what I saw in the video.
Bill
Some other things to consider when analyzing Andrius's flights:
1. GPS tends to loose nav solution when aircraft is banked. EM406 is fairly robust in this regard, but beyond a certain bank angle, it will fail. Not sure about other GPS types, but it is my impression that other types are worse in this regard. During the flight in which Andrius's plane entered a vertical dive, it was gradually increasing bank angle for sustained periods of time. Personally, I avoid more than 45 degree bank angle.
2. During sustained steep bank, and certainly during a vertical dive, GPS will "go out to lunch".
3. During vertical dive, COG is meaningless.
4. When GPS "goes out to lunch", forward (in aircraft frame of reference) acceleration information is lost, so pitch estimation will be off. In fact, during an accelerated downward dive with GPS off line, the IMU will think the plane is level!!!!
5. During an accelerated downward dive, the accelerometers are in freefall, so vertical reference information is completely gone.
For more information on some of the other effects of sustained tight turns, see my posting on this subject on diydrones.
Best regards,
Bill
lly use 2.0 or 3.0 for mixing for my flights. (Older versions of MP max out at 1.9999). Without enough roll-elevator mixing, the controls will tend to put a flying wing into a vertical spiral. Not sure what impact rudder-elevator mixing would have on a flying wing. I would hope that the code ignores is for a flying wing, but it would be best to set it to zero.
6. It is important that the roll leveling feedback gain (ROLLKP) will provide more feedback than steering control (YAWKP_AILERON). Typically, ROLLKP should be at least as large as 2 times YAWKP_AILERON. The reason why that is critical is that if ROLLKP is not big enough, the aircraft will roll more than 90 degrees in an attempt to turn. Once it goes past 90 degrees in an attempt to turn, the roll control loop will fail, the ailerons will lock up, and the aircraft will do exactly what I saw in the video.
Best regards,
Bill
- show quoted text -
Bill
Some other things to consider when analyzing Andrius's flights:
1. GPS tends to loose nav solution when aircraft is banked. EM406 is fairly robust in this regard, but beyond a certain bank angle, it will fail. Not sure about other GPS types, but it is my impression that other types are worse in this regard. During the flight in which Andrius's plane entered a vertical dive, it was gradually increasing bank angle for sustained periods of time. Personally, I avoid more than 45 degree bank angle.
2. During sustained steep bank, and certainly during a vertical dive, GPS will "go out to lunch".
3. During vertical dive, COG is meaningless.
4. When GPS "goes out to lunch", forward (in aircraft frame of reference) acceleration information is lost, so pitch estimation will be off. In fact, during an accelerated downward dive with GPS off line, the IMU will think the plane is level!!!!
5. During an accelerated downward dive, the accelerometers are in freefall, so vertical reference information is completely gone.
For more information on some of the other effects of sustained tight turns, see my posting on this subject on diydrones.
Best regards,
Bill
=== GPS ===
Andrius,
With regard to your Ublox, the fast lock is all well and good, but the question in my mind would be what compromises is it making in other areas? The important parameters with respect to the diving issue are:
1. How well does it track during high acceleration?
2. How well does it work during the combination of high bank angle and fast yaw?
Regarding the onset of the dive, it could be a bug, but it could also be the factors I listed in my previous posts.
Regarding gimbal lock, MatrixPilot does not use euler angles, it uses the elements of the direction cosine matrix directly. So gimbal lock is not possible.
Regarding other APs and flying wings, I have been working with Florin, who uses mainly flying wings with MatrixPilot for quite a while. He has not had any problems.
Regarding the definition of return to home, I think perhaps we are not on the same page. The problem is not during the flight back home, the problem is what to do afterward. What I was saying was that if the control gains are net set up in a way that would prevent a spin, the thing to do is to use more than one point in your waypoint list. It does not have to be a "big circle", it just has to be bigger than the radius of a single point, which is zero.
Wind gusts are not the problem, ROLLKP will restore ok. With respect to fighting wind gusts, by the way, the key is to make both ROLLKP and YAWKP_AILERON large, while maintaining the ratio that establishes the desired maximum roll angle.
With respect to maximum roll angle, the balance between YAWKP and ROLLKP must be reached before a bank angle of 90 degrees, otherwise the controls will roll the aircraft onto its back, and then keep going.
I think the problem in your case is likely:
1. Software/hardware bug. (I cannot rule out a problem with AUAV3.)
2. Problem with the control gain settings.
With regard to the possibility that it is the second issue, if you would be so kind as to send my your options.h file again, I will take a look to see if there is anything that might cause entry into a spiral dive while loitering a single waypoint.
Best regards,
Bill
=== Calibration ===
Andrius,
Would you please attach your latest version of options.h?
The rule of thumb to set YAWKP_AILERON equal to 1/2 * ROLLKP is necessary, but not sufficient, to get a good turn.
There are two other things that need to be adjusted to get a good, coordinated turn:
1. Both ROLLKP and YAWKP_AILERON must be large enough to be effective. In other words, think of them as ROLLP = Gain, and YAWKP = .5 * Gain, and you turn up Gain until there are roll oscillations, then back off.
2. Roll-elevator must be large enough for generate a coordinated turn. The default gain is much too low.
Anyway, if you will us your options.h file, some of the other users will probably have some suggestions.
Best regards,
Bill
Here are a couple more tips for adjusting gains.
 
1. Adjust ROLLKP in stabilized mode. Make it as large as you can without generating roll flutter.
2. Adjust YAWKP in ground tests in waypoint mode so that roll deflection goes to zero when you roll the aircraft to the attitude you would like it to have for a turn. For a 45 degree roll, YAWKP will come out to be about 0.5 of ROLLKP.
3. Adjust roll-elevator mixing in stabilized mode in ground testing to get turn coordination.
 
Best regards,
Bill
=== Roll Yaw ===
Thank you so much for your patient testing of MP AP. Clearly, you are one of our more advanced test pilots.
Its good to hear that you have confirmed the spin mechanism. From your observations it is clear that with a flying wing, if the roll feedback (ROLLKP) does not restrain the YAWKP roll feed forward before a roll angle of 90 degrees is reached, then the controls will accelerate the roll rate when the roll angle exceeds 90 degrees, because at that point the ROLLKP feedback will reduce. In other words, the result is a spinning dive, in which the FW continues to roll at a high rate. In the process, the combination of loss of yaw reference in earth frame and accumulation of roll attitude error in body frame (same as earth vertical axis) guarantees that attitude estimation will be completely wrong. Absolutely the worst thing possible.
I am very sorry about this pitfall, it will be high on my list of things to fix in the next generation of controls. There are several things planned that should completely fix the issue:
1. Unlike the present MP controls, which assume approximately level flight, the next generation controls will use the same technique that Mark Whitehorn is using in his multi-rotor branch: "virtual pose", in which any attitude can be input to the controls.
2. There will be a turn control based on classical theory, including accounting for airspeed. Part of that, of course, will be control of bank angle.
3. Implementation of fly by wire control modes.
Best regards,
Bill
https://groups.google.com/forum/m/#!topic/uavdevboard/jxyI-qdBgW8 
Piotr,
You limit bank angle in waypoint mode by adjusting ROLLKP and YAWKP_AILERON. The ratio of YAWKP_AILERON/ROLLKP determines maximum roll response to navigation control.
ROLLKP reduces bank angle response to navigation, YAWKP_AILERON increases it.
That is because in the control equation, the two terms corresponding to these two gains oppose each other.
The way to measure bank angle with a ground test is to put the controls into waypoint mode on the ground with throttle turned off. Turn the plane until you get maximum aileron deflection, and then roll the plane until the aileron deflection goes back to zero. At that point, the roll angle of the plane is maximum bank angle.
Typically you are looking for about 45 degrees. If you are getting more than that, increase ROLLKP. If you are getting less than that, increase YAWKP_AILERON.
Best regards,
Bill
With regard to FBW, there is a MP FBW control in Matt Coleman's branch.
The main branch of MP offers a stabilized mode, that works quite well, even under gusty conditions.
With regard to limiting the bank angle, I fear that I have not explained it very well. We are not limiting the control surface deflection.
On the contrary, we are increasing it. What we are doing is limiting the bank angle by increasing the control authority.
Perhaps and example would help. Start by selecting large values of ROLLKP and YAWKP_AILERON. This will have the following effect:
Under stabilized control, the ailerons will go to full deflection in response to the slightest roll disturbance, and quickly level the plane.
In waypoint mode, at the start of a turn, the ailerons will go to full deflection. The plane will then rotate as fast as possible, until it reaches the maximum bank angle. At that point, the ailerons will return to neutral.
The maximum waypoint bank is defined by YAWKP_AILERON*(yaw_error) + ROLLKP*(bank_angle) = 0.
The maximum yaw error at the beginning of a turn is about 90 degrees.
So, bank_angle = - (YAWKP_AILERON/ROLLKP)*90
For a bank angle of 45 degrees during a waypoint turn, set ROLLKP = 2* YAWKP_AILERON
Now, back to the question of response of the controls to wind gusts. The best way to do that is to make ROLLKP as large as you can without generating roll instability.
For more information on setting MP gains, if you have not already done so, I suggest you take a look here.
As far as roll control is concerned, here is what I suggest to do to set ROLLKP and YAWKP_AILERON:
1. Flying in stabilized mode only, raise ROLLKP until roll control begins to flutter. (Flutter amplitude will be self limiting, so don't panic when you see the fluttering.) The point of raising ROLLKP is that it provides control authority. Basically, if ROLLKP is large enough, wind gusts have no effects.
Raising ROLLKP will tend to make the plane fly level all of the time. You might have trouble getting the plane to turn. In that case, you will need to set a value for AILERON_BOOST, typically 0.5 to 1.0 will work.
2. Set YAWKP_AILERON to 0.5 of ROLLKP. Do a ground test in waypoint mode to see what the bank angle will be when yaw control and roll control are in balance.
Best regards,
Bill
A couple more tips for setting ROLLKP and YAWKP_AILERON.
Start with ground testing in stabilized mode. Roll the plane and observe the deflection of the ailerons. Compare it with how much deflection you think you would use to control roll. Then raise or lower ROLLKP accordingly. Since the IMU based controls can react quickly, there is less of an issue of overcontrolling, so you should set the deflection to be much larger than you would ordinarily use.
I have helped several members of my flying club set up their UDBs, and found that there was a wide variation in the mechanical gain of linkages between servos and control surfaces. So, there was a wide variation in the required values of ROLLKP.
Once you have established ROLLKP, a good starting point for YAWKP_AILERON is 0.5*ROLLKP.
Best regards,
Bill
Category:Gentlenav gimbal
Category:Gentlenav hilsim
Category:Gentlenav calibration
Category:Gentlenav magnetometer
Category:Gentlenav Roll_yaw
Category:Gimbal