Dive after roll from inverted to uninverted

I have an elevon controlled flying wing and was experimenting with inverted flight, and experienced some unusual behavior when rolling from inverted back to uninverted in FBWA. While rolling, the plane applied full elevator which, because it was nearly sideways, resulted in a large earth-frame yaw, followed by a dive which the autopilot did nothing to correct (it actually applied slight down elevator). Unfortunately I was doing this at too low an altitude, so I didn’t have time to manually recover before it crashed.

Here is the log file: https://drive.google.com/file/d/1vXEgGlLQ2lP2xr6Mb01U0zXeA2HKcM0h/view?usp=sharing

I’ve been reviewing the log and the code, and have identified some factors that I think contributed to the crash:

  • The turn coordination function (AP_PitchController::_get_coordination_rate_offset()) applies an offset to the requested pitch rate that is not bounded by the pitch rate limits (see: AP_PitchController::get_servo_out()). In this case it resulted in a maximum requested pitch rate of 486 deg/sec, even though the limit is set to 75 deg/dec, which in turn resulted in saturation of the elevator. As the plane finished the roll, this extreme rate request actually caused it to start to pull up, but then the pitch passed -70 degrees, which disabled the turn coordination.
  • There is logic to reduce the elevator deflection at high bank angles (see: https://github.com/ArduPilot/ardupilot/blob/ArduPlane-4.0.7/libraries/APM_Control/AP_PitchController.cpp#L215). This is designed to prevent the earth-frame yaw I observed, but this didn’t seem to have any effect here. The comment says that the elevator deflection will be reduced to zero at 90 degrees bank, but in reality it will only be reduced to ~11% of the original value. I suspect that the extreme pitch rate request caused the elevator to saturate even when it was scaled down.
  • As the plane was diving, the autopilot did not make any attempt to pull out of the dive. In fact, the elevator deflection was slightly negative (1-2 degrees) most of the time. This was caused by integrator windup which occurred due to the extreme negative pitch rates requested by the turn coordinator while the plane was inverted.

I believe the root of the problem lies in the turn coordinator. It doesn’t make sense to me to try to make a coordinated turn while rolling to/from inverted. It seems like we should have some way of distinguishing between coordinated and uncoordinated maneuvers, although I’m not sure how this would be implemented. Also, I don’t think the turn coordinator should be allowed to exceed the pitch rate limits.

Also, it seems like there is a bit of mixing of coordinate frames in the pitch controller. My understanding is that the pitch controller tries to maintain pitch angle in the earth frame, but it does that by controlling the elevator, which controls the body frame pitch axis. To compensate for this, it reduces the elevator deflection at high bank angles, but this seems like a crude workaround for the real problem. It seems like it would be better to rotate the earth frame target attitude into the body frame before giving it to the PID controllers. This would inherently prevent the elevator from being used for earth frame pitch control at high bank angles (but it would still be used for turn coordination) and potentially enable interesting 3D control.

I’m really curious what kind of flying wing this is? The speeds, both GPS and CTUN.Airspeed both seem really slow to me.

It is based on this design: https://www.rcgroups.com/forums/showthread.php?1048425-48-Divinity-II-Build***Now-with-Proof-of-flight-video***

I implemented some fixes for the issues I identified in this branch: Commits · lopsided98/ardupilot · GitHub. I still don’t feel safe testing inverted flight without fixing the root cause turn coordination issue though.

Would be great to see a PR for some of these improvements, probably also the best way to get a few of the devs to have a look. You can always open as draft initially.

I spied some sailboat improvements too, would be great to get them in!

I created a PR for the simplest of those patches a while ago (intended in part as a way to start the discussion about the other changes), but didn’t get any feedback: AP_Control: apply pitch rate limit to turn coordination by lopsided98 · Pull Request #16957 · ArduPilot/ardupilot · GitHub