Gravity Alignment v26

/*
/// Whip's Gravity Alignment Systems v26 - revision: 9/22/18 ///

Written by Whiplash141
*/

/*
==============================
    You can edit these vars
==============================
*/

const string gyroExcludeName = "Exclude";
const string statusScreenName = "Alignment"; //(Optional) Name of status screen
const string shipName = "\nPFS - Nightjar Orbital Bomber"; //(Optional) Name of your ship

bool shouldAlign = true; //If the script should attempt to stabalize by default
bool referenceOnSameGridAsProgram = true; //if true, only searches for reference blocks on
                                          //the same grid as the program block (should help with docking small vessels)

const double angleTolerance = 0; //How many degrees the code will allow before it overrides user control

//---PID Constants
const double proportionalConstant = 2;
const double derivativeConstant = .5;

/*
====================================================
    Don't touch anything below this <3 - Whiplash
====================================================
*/

const double updatesPerSecond = 10;
const double timeFlashMax = .5; //in seconds
const double timeLimit = 1 / updatesPerSecond;
double angleRoll = 0;
double anglePitch = 0;
double timeElapsed = 0;
bool canTolerate = true;
string stableStatus = ">> Disabled <<";
string gravityMagnitudeString;
string overrideStatus;

List<IMyGyro> gyros = new List<IMyGyro>();
List<IMyShipController> shipControllers = new List<IMyShipController>();

PID pitchPID;
PID rollPID;

Program()
{
    Runtime.UpdateFrequency = UpdateFrequency.Once;
    Echo("If you can read this\nclick the 'Run' button!");
    pitchPID = new PID(proportionalConstant, 0, derivativeConstant, -10, 10, timeLimit);
    rollPID = new PID(proportionalConstant, 0, derivativeConstant, -10, 10, timeLimit);
}

void Main(string arg, UpdateType updateSource)
{
    //------------------------------------------
    //This is a bandaid
    if ((Runtime.UpdateFrequency & UpdateFrequency.Update1) == 0)
        Runtime.UpdateFrequency = UpdateFrequency.Update1;
    //------------------------------------------

    timeElapsed += Runtime.TimeSinceLastRun.TotalSeconds;

    switch (arg.ToLower())
    {
        case "toggle":
            if (!shouldAlign)
            {
                shouldAlign = true;
                stableStatus = "<< Active >>";
            }
            else
            {
                shouldAlign = false;
                stableStatus = ">> Disabled <<";
            }
            break;

        case "on":
            shouldAlign = true;
            stableStatus = "<< Active >>";
            break;

        case "off":
            shouldAlign = false;
            stableStatus = ">> Disabled <<";
            break;

        default:
            break;
    }

    if (timeElapsed >= timeLimit)
    {
        AlignWithGravity();
        StatusScreens();
        timeElapsed = 0;
        Echo("Stabilizers on?: " + shouldAlign.ToString());
    }
}

bool ShouldFetch(IMyTerminalBlock block)
{
    if (block is IMyShipController)
    {
        if (referenceOnSameGridAsProgram)
        {
            return block.CubeGrid == Me.CubeGrid;
        }
        else
        {
            return true;
        }
    }
    else
    {
        return false;
    }
}

IMyShipController GetControlledShipController(List<IMyShipController> controllers)
{
    if (controllers.Count == 0)
        return null;

    foreach (IMyShipController thisController in controllers)
    {
        if (thisController.IsUnderControl && thisController.CanControlShip)
            return thisController;
    }

    return controllers[0];
}

void AlignWithGravity()
{
    //---Find our refrence and comparision blocks
    GridTerminalSystem.GetBlocksOfType(shipControllers, ShouldFetch);

    //---Check for any cases that would lead to code failure
    if (shipControllers.Count == 0)
    {
        Echo($"ERROR: No ship controller was found");
        return;
    }

    //---Assign our reference block
    IMyShipController referenceBlock = GetControlledShipController(shipControllers);

    //---Populate gyro list
    gyros.Clear();
    GridTerminalSystem.GetBlocksOfType(gyros, block => block.CubeGrid == referenceBlock.CubeGrid && !block.CustomName.Contains(gyroExcludeName));

    if (gyros.Count == 0)
    {
        Echo("ERROR: No gyros found on ship");
        return;
    }

    //---Get gravity vector
    var referenceOrigin = referenceBlock.GetPosition();
    var gravityVec = referenceBlock.GetNaturalGravity();
    var gravityVecLength = gravityVec.Length();
    gravityMagnitudeString = Math.Round(gravityVecLength, 2).ToString() + " m/s²";
    if (gravityVec.LengthSquared() == 0)
    {
        gravityMagnitudeString = "No Gravity";

        foreach (IMyGyro thisGyro in gyros)
        {
            thisGyro.SetValue("Override", false);
        }
        overrideStatus = "";
        stableStatus = ">> Disabled <<";

        shouldAlign = false;

        angleRoll = 0; angleRoll = 0;
        return;
    }

    Vector3D leftVec = Vector3D.Cross(-gravityVec, referenceBlock.WorldMatrix.Forward);
    Vector3D forwardVec = Vector3D.Cross(gravityVec, leftVec);

    double temp; //not used for anything
    GetRotationAngles(forwardVec, -gravityVec, referenceBlock.WorldMatrix, out temp, out anglePitch, out angleRoll);

    Echo("pitch angle:" + Math.Round((anglePitch / Math.PI * 180), 2).ToString() + " deg");
    Echo("roll angle:" + Math.Round((angleRoll / Math.PI * 180), 2).ToString() + " deg");


    //---Get Raw Deviation angle
    double rawDevAngle = Math.Acos(MathHelper.Clamp(gravityVec.Dot(referenceBlock.WorldMatrix.Forward) / gravityVec.Length() * 180 / Math.PI, -1, 1));

    //---Angle controller
    double rollSpeed = rollPID.Control(angleRoll); //Math.Round(angleRoll * proportionalConstant + (angleRoll - lastAngleRoll) / timeElapsed * derivativeConstant, 2);
    double pitchSpeed = pitchPID.Control(anglePitch); //Math.Round(anglePitch * proportionalConstant + (anglePitch - lastAnglePitch) / timeElapsed * derivativeConstant, 2);                                                                                                                                                            //w.H]i\p

    var mouseInput = referenceBlock.RotationIndicator;

    //rollSpeed = rollSpeed / gyros.Count;
    //pitchSpeed = pitchSpeed / gyros.Count;

    //---Check if we are inside our tolerances
    canTolerate = true;

    if (Math.Abs(anglePitch * 180 / Math.PI) > angleTolerance)
    {
        canTolerate = false;
    }

    if (Math.Abs(angleRoll * 180 / Math.PI) > angleTolerance)
    {
        canTolerate = false;
    }

    //---Set appropriate gyro override
    if (shouldAlign && !canTolerate)
    {
        //do gyros
        ApplyGyroOverride(mouseInput.Y, pitchSpeed, rollSpeed, gyros, referenceBlock.WorldMatrix);

        overrideStatus = $"\n\n           SAFETY OVERRIDE ACTIVE"; //\nYaw : {yawSpeed}";

        /*timeFlash += timeElapsed;
        if (timeFlash > timeFlashMax)
        {
            if (flashOn)
            {
                overrideStatus = "\n\n           SAFETY OVERRIDE ACTIVE";
                flashOn = false;
            }
            else
            {
                overrideStatus = "";
                flashOn = true;
            }
            timeFlash = 0;
        }*/
    }
    else
    {
        foreach (IMyGyro thisGyro in gyros)
        {
            thisGyro.SetValue("Override", false);
        }
        overrideStatus = "";
    }
}

/*
/// Whip's Get Rotation Angles Method v16 - 9/25/18 ///
Dependencies: VectorMath
Note: Set desiredUpVector to Vector3D.Zero if you don't care about roll
*/
void GetRotationAngles(Vector3D desiredForwardVector, Vector3D desiredUpVector, MatrixD worldMatrix, out double yaw, out double pitch, out double roll)
{
    var localTargetVector = Vector3D.Rotate(desiredForwardVector, MatrixD.Transpose(worldMatrix));
    var flattenedTargetVector = new Vector3D(localTargetVector.X, 0, localTargetVector.Z);

    yaw = VectorMath.AngleBetween(Vector3D.Forward, flattenedTargetVector) * Math.Sign(localTargetVector.X); //right is positive
    if (Math.Abs(yaw) < 1E-6 && localTargetVector.Z > 0) //check for straight back case
        yaw = Math.PI;

    if (Vector3D.IsZero(flattenedTargetVector)) //check for straight up case
        pitch = MathHelper.PiOver2 * Math.Sign(localTargetVector.Y);
    else
        pitch = VectorMath.AngleBetween(localTargetVector, flattenedTargetVector) * Math.Sign(localTargetVector.Y); //up is positive

    if (Vector3D.IsZero(desiredUpVector))
    {
        roll = 0;
        return;
    }
    var localUpVector = Vector3D.Rotate(desiredUpVector, MatrixD.Transpose(worldMatrix));
    var flattenedUpVector = new Vector3D(localUpVector.X, localUpVector.Y, 0);
    roll = VectorMath.AngleBetween(flattenedUpVector, Vector3D.Up) * Math.Sign(Vector3D.Dot(Vector3D.Right, flattenedUpVector));
}

void StatusScreens()
{
    //---get the parts of our string
    double roll_deg = angleRoll / Math.PI * 180;
    double pitch_deg = -anglePitch / Math.PI * 180;
    string rollStatusString = AngleStatus(roll_deg);
    string pitchStatusString = AngleStatus(pitch_deg);

    //---Construct our final string
    string statusScreenMessage = shipName
        + "\nNatural Gravity: " + gravityMagnitudeString
        + "\nStabilizer: " + stableStatus
        + "\n\nRoll Angle: " + Math.Round(roll_deg, 2).ToString() + " degrees\n" + rollStatusString
        + "\n\nPitch Angle: " + Math.Round(pitch_deg, 2).ToString() + " degrees\n" + pitchStatusString
        + overrideStatus;


    //---Write to screens
    var screens = new List<IMyTerminalBlock>();
    GridTerminalSystem.SearchBlocksOfName(statusScreenName, screens, block => block is IMyTextPanel);

    if (screens.Count == 0)
        return;

    foreach (IMyTextPanel thisScreen in screens)
    {
        var fontSize = thisScreen.FontSize;
        //thisScreen.Font = "Monospace";
        var wrappedText = TextHelper.CenterTextMonospace(statusScreenMessage, fontSize);

        thisScreen.WritePublicText(wrappedText);
        if (!thisScreen.ShowText)
            thisScreen.ShowPublicTextOnScreen();
    }
}

const string align_15 = "[-15](-)-------0----------[+15]";
const string align_14 = "[-15]-(-)------0----------[+15]";
const string align_12 = "[-15]--(-)-----0----------[+15]";
const string align_10 = "[-15]---(-)----0----------[+15]";
const string align_8 =  "[-15]----(-)---0----------[+15]";
const string align_6 =  "[-15]-----(-)--0----------[+15]";
const string align_4 =  "[-15]------(-)-0----------[+15]";
const string align_2 =  "[-15]-------(-)0----------[+15]";
const string align0 =   "[-15]---------(0)---------[+15]";
const string align2 =   "[-15]----------0(-)-------[+15]";
const string align4 =   "[-15]----------0-(-)------[+15]";
const string align6 =   "[-15]----------0--(-)-----[+15]";
const string align8 =   "[-15]----------0---(-)----[+15]";
const string align10 =  "[-15]----------0----(-)---[+15]";
const string align12 =  "[-15]----------0-----(-)--[+15]";
const string align14 =  "[-15]----------0------(-)-[+15]";
const string align15 =  "[-15]----------0-------(-)[+15]";

string AngleStatus(double angle)
{
    if (angle > 15)
        return align15;
    else if (angle > 14)
        return align14;
    else if (angle > 12)
        return align12;
    else if (angle > 10)
        return align10;
    else if (angle > 8)
        return align8;
    else if (angle > 6)
        return align6;
    else if (angle > 4)
        return align4;
    else if (angle > 2)
        return align2;
    else if (angle > -2)
        return align0;
    else if (angle > -4)
        return align_2;
    else if (angle > -6)
        return align_4;
    else if (angle > -8)
        return align_6;
    else if (angle > -10)
        return align_8;
    else if (angle > -12)
        return align_10;
    else if (angle > -14)
        return align_12;
    else if (angle > -15)
        return align_14;
    else
        return align_15;
}

Vector3D VectorProjection(Vector3D a, Vector3D b) //proj a on b
{
    Vector3D projection = a.Dot(b) / b.LengthSquared() * b;
    return projection;
}

int VectorCompareDirection(Vector3D a, Vector3D b) //returns -1 if vectors return negative dot product
{
    double check = a.Dot(b);
    if (check < 0)
        return -1;
    else
        return 1;
}

double VectorAngleBetween(Vector3D a, Vector3D b) //returns radians
{
    if (a.LengthSquared() == 0 || b.LengthSquared() == 0)
        return 0;
    else
        return Math.Acos(MathHelper.Clamp(a.Dot(b) / a.Length() / b.Length(), -1, 1));
}

//Whip's ApplyGyroOverride Method v11 - 3/22/19
void ApplyGyroOverride(double yawSpeed, double pitchSpeed, double rollSpeed, List<IMyGyro> gyroList, MatrixD worldMatrix)
{
    var rotationVec = new Vector3D(-pitchSpeed, yawSpeed, rollSpeed); //because keen does some weird stuff with signs
    var relativeRotationVec = Vector3D.TransformNormal(rotationVec, worldMatrix);

    foreach (var thisGyro in gyroList)
    {
        var transformedRotationVec = Vector3D.TransformNormal(relativeRotationVec, Matrix.Transpose(thisGyro.WorldMatrix));

        thisGyro.Pitch = (float)transformedRotationVec.X;
        thisGyro.Yaw = (float)transformedRotationVec.Y;
        thisGyro.Roll = (float)transformedRotationVec.Z;
        thisGyro.GyroOverride = true;
    }
}

public static class VectorMath
{
    /// <summary>
    ///  Normalizes a vector only if it is non-zero and non-unit
    /// </summary>
    public static Vector3D SafeNormalize(Vector3D a)
    {
        if (Vector3D.IsZero(a))
            return Vector3D.Zero;

        if (Vector3D.IsUnit(ref a))
            return a;

        return Vector3D.Normalize(a);
    }

    /// <summary>
    /// Reflects vector a over vector b with an optional rejection factor
    /// </summary>
    public static Vector3D Reflection(Vector3D a, Vector3D b, double rejectionFactor = 1) //reflect a over b
    {
        Vector3D project_a = Projection(a, b);
        Vector3D reject_a = a - project_a;
        return project_a - reject_a * rejectionFactor;
    }

    /// <summary>
    /// Rejects vector a on vector b
    /// </summary>
    public static Vector3D Rejection(Vector3D a, Vector3D b) //reject a on b
    {
        if (Vector3D.IsZero(a) || Vector3D.IsZero(b))
            return Vector3D.Zero;

        return a - a.Dot(b) / b.LengthSquared() * b;
    }

    /// <summary>
    /// Projects vector a onto vector b
    /// </summary>
    public static Vector3D Projection(Vector3D a, Vector3D b)
    {
        if (Vector3D.IsZero(a) || Vector3D.IsZero(b))
            return Vector3D.Zero;

        return a.Dot(b) / b.LengthSquared() * b;
    }

    /// <summary>
    /// Scalar projection of a onto b
    /// </summary>
    public static double ScalarProjection(Vector3D a, Vector3D b)
    {
        if (Vector3D.IsZero(a) || Vector3D.IsZero(b))
            return 0;

        if (Vector3D.IsUnit(ref b))
            return a.Dot(b);

        return a.Dot(b) / b.Length();
    }

    /// <summary>
    /// Computes angle between 2 vectors
    /// </summary>
    public static double AngleBetween(Vector3D a, Vector3D b) //returns radians
    {
        if (Vector3D.IsZero(a) || Vector3D.IsZero(b))
            return 0;
        else
            return Math.Acos(MathHelper.Clamp(a.Dot(b) / Math.Sqrt(a.LengthSquared() * b.LengthSquared()), -1, 1));
    }

    /// <summary>
    /// Computes cosine of the angle between 2 vectors
    /// </summary>
    public static double CosBetween(Vector3D a, Vector3D b, bool useSmallestAngle = false) //returns radians
    {
        if (Vector3D.IsZero(a) || Vector3D.IsZero(b))
            return 0;
        else
            return MathHelper.Clamp(a.Dot(b) / Math.Sqrt(a.LengthSquared() * b.LengthSquared()), -1, 1);
    }

    /// <summary>
    /// Returns if the normalized dot product between two vectors is greater than the tolerance.
    /// This is helpful for determining if two vectors are "more parallel" than the tolerance.
    /// </summary>
    /// <param name="a"></param>
    /// <param name="b"></param>
    /// <param name="tolerance"></param>
    /// <returns></returns>
    public static bool IsDotProductWithinTolerance(Vector3D a, Vector3D b, double tolerance)
    {
        double dot = Vector3D.Dot(a, b);
        double num = a.LengthSquared() * b.LengthSquared() * tolerance * tolerance;
        return dot * dot > num;
    }
}

//Whip's PID controller class v6 - 11/22/17
public class PID
{
    double _kP = 0;
    double _kI = 0;
    double _kD = 0;
    double _integralDecayRatio = 0;
    double _lowerBound = 0;
    double _upperBound = 0;
    double _timeStep = 0;
    double _inverseTimeStep = 0;
    double _errorSum = 0;
    double _lastError = 0;
    bool _firstRun = true;
    bool _integralDecay = false;
    public double Value { get; private set; }

    public PID(double kP, double kI, double kD, double lowerBound, double upperBound, double timeStep)
    {
        _kP = kP;
        _kI = kI;
        _kD = kD;
        _lowerBound = lowerBound;
        _upperBound = upperBound;
        _timeStep = timeStep;
        _inverseTimeStep = 1 / _timeStep;
        _integralDecay = false;
    }

    public PID(double kP, double kI, double kD, double integralDecayRatio, double timeStep)
    {
        _kP = kP;
        _kI = kI;
        _kD = kD;
        _timeStep = timeStep;
        _inverseTimeStep = 1 / _timeStep;
        _integralDecayRatio = integralDecayRatio;
        _integralDecay = true;
    }

    public double Control(double error)
    {
        //Compute derivative term
        var errorDerivative = (error - _lastError) * _inverseTimeStep;

        if (_firstRun)
        {
            errorDerivative = 0;
            _firstRun = false;
        }

        //Compute integral term
        if (!_integralDecay)
        {
            _errorSum += error * _timeStep;

            //Clamp integral term
            if (_errorSum > _upperBound)
                _errorSum = _upperBound;
            else if (_errorSum < _lowerBound)
                _errorSum = _lowerBound;
        }
        else
        {
            _errorSum = _errorSum * (1.0 - _integralDecayRatio) + error * _timeStep;
        }

        //Store this error as last error
        _lastError = error;

        //Construct output
        this.Value = _kP * error + _kI * _errorSum + _kD * errorDerivative;
        return this.Value;
    }

    public double Control(double error, double timeStep)
    {
        _timeStep = timeStep;
        _inverseTimeStep = 1 / _timeStep;
        return Control(error);
    }

    public void Reset()
    {
        _errorSum = 0;
        _lastError = 0;
        _firstRun = true;
    }
}

// Whip's TextHelper Class v2
public class TextHelper
{
    static StringBuilder textSB = new StringBuilder();
    const float adjustedPixelWidth = (512f / 0.778378367f);
    const int monospaceCharWidth = 24 + 1; //accounting for spacer
    const int spaceWidth = 8;

    #region Default font
        #region bigass dictionary
    static Dictionary<char, int> _charWidths = new Dictionary<char, int>()
    {
{'.', 9},
{'!', 8},
{'?', 18},
{',', 9},
{':', 9},
{';', 9},
{'"', 10},
{'\'', 6},
{'+', 18},
{'-', 10},

{'(', 9},
{')', 9},
{'[', 9},
{']', 9},
{'{', 9},
{'}', 9},

{'\\', 12},
{'/', 14},
{'_', 15},
{'|', 6},

{'~', 18},
{'<', 18},
{'>', 18},
{'=', 18},

{'0', 19},
{'1', 9},
{'2', 19},
{'3', 17},
{'4', 19},
{'5', 19},
{'6', 19},
{'7', 16},
{'8', 19},
{'9', 19},

{'A', 21},
{'B', 21},
{'C', 19},
{'D', 21},
{'E', 18},
{'F', 17},
{'G', 20},
{'H', 20},
{'I', 8},
{'J', 16},
{'K', 17},
{'L', 15},
{'M', 26},
{'N', 21},
{'O', 21},
{'P', 20},
{'Q', 21},
{'R', 21},
{'S', 21},
{'T', 17},
{'U', 20},
{'V', 20},
{'W', 31},
{'X', 19},
{'Y', 20},
{'Z', 19},

{'a', 17},
{'b', 17},
{'c', 16},
{'d', 17},
{'e', 17},
{'f', 9},
{'g', 17},
{'h', 17},
{'i', 8},
{'j', 8},
{'k', 17},
{'l', 8},
{'m', 27},
{'n', 17},
{'o', 17},
{'p', 17},
{'q', 17},
{'r', 10},
{'s', 17},
{'t', 9},
{'u', 17},
{'v', 15},
{'w', 27},
{'x', 15},
{'y', 17},
{'z', 16}
    };
        #endregion

    public static int GetWordWidth(string word)
    {
        int wordWidth = 0;
        foreach(char c in word)
        {
            int thisWidth = 0;
            bool contains = _charWidths.TryGetValue(c, out thisWidth);
            if (!contains)
                thisWidth = monospaceCharWidth; //conservative estimate

            wordWidth += thisWidth;
        }
        return wordWidth;
    }

    public static string WrapText(string text, float fontSize, float pixelWidth = adjustedPixelWidth)
    {
        textSB.Clear();
        var words = text.Split(' ');
        var screenWidth = (pixelWidth / fontSize);
        int currentLineWidth = 0;
        foreach (var word in words)
        {
            if (currentLineWidth == 0)
            {
                textSB.Append($"{word}");
                currentLineWidth += GetWordWidth(word);
                continue;
            }

            currentLineWidth += spaceWidth + GetWordWidth(word);
            if (currentLineWidth > screenWidth) //new line
            {
                currentLineWidth = GetWordWidth(word);
                textSB.Append($"\n{word}");
            }
            else
            {
                textSB.Append($" {word}");
            }
        }

        return textSB.ToString();
    }
    #endregion

    #region Monospace
    public static float GetMinimumFontSizeMonospace(int textCharacters)
    {
        var pixelWidth = textCharacters * monospaceCharWidth;
        return adjustedPixelWidth / pixelWidth;
    }

    public static string WrapTextMonospace(string text, float fontSize)
    {
        textSB.Clear();
        var words = text.Split(' ');
        var screenWidth = (adjustedPixelWidth / fontSize);
        int currentLineWidth = 0;
        foreach (var word in words)
        {
            if (currentLineWidth == 0)
            {
                textSB.Append($"{word}");
                currentLineWidth += word.Length * monospaceCharWidth;
                continue;
            }

            currentLineWidth += (1 + word.Length) * monospaceCharWidth;
            if (currentLineWidth > screenWidth) //new line
            {
                currentLineWidth = word.Length * monospaceCharWidth;
                textSB.Append($"\n{word}");
            }
            else
            {
                textSB.Append($" {word}");
            }

        }
        return textSB.ToString();
    }

    public static string CenterTextMonospace(string wrappedText, float fontSize)
    {
        textSB.Clear();
        var lines = wrappedText.Split('\n');
        var screenWidth = (adjustedPixelWidth / fontSize);
        var maxCharsPerLine = Math.Floor(screenWidth / monospaceCharWidth);

        foreach (var line in lines)
        {
            var trimmedLine = line.Trim();
            var charCount = trimmedLine.Length;
            var diff = maxCharsPerLine - charCount;
            var halfDiff = (int)Math.Max(diff / 2, 0);
            textSB.Append(new string(' ', halfDiff)).Append(trimmedLine).Append("\n");
        }
        return textSB.ToString();
    }

    public static string RightJustifyMonospace(string wrappedText, float fontSize)
    {
        textSB.Clear();
        var lines = wrappedText.Split('\n');
        var screenWidth = (adjustedPixelWidth / fontSize);
        var maxCharsPerLine = (int)Math.Floor(screenWidth / monospaceCharWidth);

        foreach (var line in lines)
        {
            var trimmedLine = line.Trim();
            var charCount = trimmedLine.Length;
            var diff = maxCharsPerLine - charCount;
            diff = (int)Math.Max(0, diff);
            textSB.Append(new string(' ', diff)).Append(trimmedLine).Append("\n");
        }
        return textSB.ToString();
    }
    #endregion
}


/*
/// WHAT'S CHANGED? ///
* Ignore offgrid gyros
* Fixed angles spazzing out
* Added yaw control while safety override is active - v23
* Removed the need for a name tag for the control seat - v24
* Added gyro exclude name tag - v25
*/