Created
October 5, 2022 01:32
-
-
Save brihernandez/ed1b2d85cdd9811b07824d9e68af7875 to your computer and use it in GitHub Desktop.
I wrote a Stability Augmentation System once. I forgot what I read that explained it, but I was really surprised how effective it was at stabilizing a Halo Pelican. I ended up not needing a PID for the stick and rudder, since this kept the oscillations under control.
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
// This is a basic dummy version of a Stability Augmentation System. There's a bunch of technical | |
// terms for this, but I'm not a control systems engineer so feel free to correct me. I found a | |
// really awesome document (video?) somewhere that explained all this stuff but it was a while ago | |
// but I forgot where I saw it. | |
// This is meant to be set up per channel. It's serializable so you can change | |
// the authority and power of it per channel in the Inspector. | |
[System.Serializable] | |
public class SASChannel | |
{ | |
[Tooltip("How aggressive the SAS is in stabilization.")] | |
public float Power = 10f; | |
[Tooltip("How much control authority the AI is allowed. E.g. a value of 1 can command the full range of an input axis in an attempt to stabilize.")] | |
[Range(0, 1)] public float Authority = .1f; | |
public enum Channel | |
{ | |
Pitch, | |
Yaw, | |
Roll, | |
} | |
public float GetDeflection(Vector3 localAngularVelocity, float stickDeflection, Channel channel) | |
{ | |
var sasIntent = channel switch | |
{ | |
Channel.Pitch => localAngularVelocity.x, | |
Channel.Yaw => localAngularVelocity.y, | |
Channel.Roll => localAngularVelocity.z, | |
_ => 0f, | |
}; | |
sasIntent *= Power; | |
// Washout filter so that the pilot can override the SAS at high stick deflections. | |
// Otherwise, at full stick deflection, the SAS will fight back to prevent turns. | |
var authority = Authority * (1f - Mathf.Abs(stickDeflection)); | |
sasIntent = Mathf.Clamp(sasIntent, -authority, authority); | |
return sasIntent; | |
} | |
} | |
// Here's an example of a very, very basic class using the three channels of SAS. Assume that the | |
// the pitch/yaw/roll variables are being set by some controller somewhere, and operate on a range | |
// of -1, to 1. | |
public class YourCoolAirplaceSpaceship : MonoBehaviour | |
{ | |
public SASChannel SASChannelPitch = new SASChannel(); | |
public SASChannel SASChannelRoll = new SASChannel(); | |
public SASChannel SASChannelYaw = new SASChannel(); | |
public float pitch = 0f; | |
public float yaw = 0f; | |
public float roll = 0f; | |
private void Update() | |
{ | |
// The local angular velocity allows you to tell how quickly you're pitching/yawing/rolling | |
// relative to your own airframe. | |
var localAngularVelocity = transform.InverseTransformDirection(Rigidbody.angularVelocity); | |
// The SAS will give its own commands to the axis it's told to. Adding its command to your | |
// commands will cause it to have enough control over the stick to keep the craft stable. | |
// Remember to clamp to prevent inputs greater than a normalized value! | |
var sasPitchDeflection = SASChannelPitch.GetDeflection(localAngularVelocity, pitch, SASChannel.Channel.Pitch); | |
pitch -= sasPitchDeflection; | |
pitch = Mathf.Clamp(pitch, -1f, 1f); | |
var sasRollDeflection = SASChannelRoll.GetDeflection(localAngularVelocity, roll, SASChannel.Channel.Roll); | |
roll += sasRollDeflection; | |
roll = Mathf.Clamp(roll, -1f, 1f); | |
var sasYawDeflection = SASChannelYaw.GetDeflection(localAngularVelocity, yaw, SASChannel.Channel.Yaw); | |
yaw -= sasYawDeflection; | |
yaw = Mathf.Clamp(yaw, -1f, 1f); | |
} | |
} |
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment