AFCS.cpp

Go to the documentation of this file.

#include "Module.h"
#include "../Extensions.h"

Afcs::Afcs(Bus* prmBus, Logger* prmLogger) : Module(prmBus)
{
        bus = prmBus;
        logger = prmLogger;
}

void Afcs::FrameMove(float fElapsedTime)
{

#pragma region Commands

        for (UINT i = 0; i < bus->commandStream.size(); i++)
        {
                Command command = bus->commandStream.at(i);
                if (command.delay != 0.0f) continue;

                if (command.name == "AFCS-VerticalModeCycle")
                {
                        Command newCommand;
                        newCommand.name = "AFCS-VerticalMode";
                        switch (bus->AFCS.CurrentVerticalMode)
                        {
                        case Bus::Afcs::VerticalModes::VerticalMode_Off: newCommand.ivalue = static_cast<int>(Bus::Afcs::VerticalModes::Pitch);
                                break;
                        case Bus::Afcs::VerticalModes::Pitch: newCommand.ivalue = static_cast<int>(Bus::Afcs::VerticalModes::VerticalSpeed);
                                break;
                        case Bus::Afcs::VerticalModes::VerticalSpeed: newCommand.ivalue = static_cast<int>(Bus::Afcs::VerticalModes::Altitude);
                                break;
                        default: newCommand.ivalue = static_cast<int>(Bus::Afcs::VerticalModes::VerticalMode_Off);
                                break;
                        }
                        bus->commandStream.push_back(newCommand);
                        bus->commandStream.erase(bus->commandStream.begin() + i);
                }
                else if (command.name == "AFCS-LateralModeCycle")
                {
                        Command newCommand;
                        newCommand.name = "AFCS-LateralMode";
                        switch (bus->AFCS.CurrentLateralMode)
                        {
                        case Bus::Afcs::LateralModes::LateralMode_Off: newCommand.ivalue = static_cast<int>(Bus::Afcs::LateralModes::Roll);
                                break;
                        case Bus::Afcs::LateralModes::Roll: newCommand.ivalue = static_cast<int>(Bus::Afcs::LateralModes::Heading);
                                break;
                        case Bus::Afcs::LateralModes::Heading:
                                {
                                        if (bus->waypoints.size() > 0) // must have a waypoint in order to NAV
                                                newCommand.ivalue = static_cast<int>(Bus::Afcs::LateralModes::Navigation);
                                        else
                                                newCommand.ivalue = static_cast<int>(Bus::Afcs::LateralModes::LateralMode_Off);
                                        break;
                                }
                        default: newCommand.ivalue = static_cast<int>(Bus::Afcs::LateralModes::LateralMode_Off);
                                break;
                        }
                        bus->commandStream.push_back(newCommand);
                        bus->commandStream.erase(bus->commandStream.begin() + i);
                }
                else if (command.name == "AFCS-VerticalMode")
                {
                        bus->AFCS.CurrentVerticalMode = static_cast<Bus::Afcs::VerticalModes>(command.ivalue);

                        if (bus->AFCS.CurrentVerticalMode && bus->AFCS.CurrentLateralMode == Bus::Afcs::LateralModes::LateralMode_Off)
                        {
                                Command newCommand;
                                newCommand.name = "AFCS-LateralMode";
                                newCommand.ivalue = static_cast<int>(Bus::Afcs::LateralModes::Roll);
                                bus->commandStream.push_back(newCommand);
                        }

                        if (!bus->AFCS.CurrentVerticalMode)
                        {
                                if (bus->AFCS.CurrentLateralMode)
                                {
                                        Command newCommand;
                                        newCommand.name = "AFCS-LateralMode";
                                        newCommand.ivalue = static_cast<int>(Bus::Afcs::LateralModes::LateralMode_Off);
                                        bus->commandStream.push_back(newCommand);
                                }
                                if (bus->AFCS.AutopilotEngaged)
                                {
                                        Command newCommand;
                                        newCommand.name = "AFCS-Autopilot";
                                        newCommand.bvalue = false;
                                        bus->commandStream.push_back(newCommand);
                                }
                                if (bus->AFCS.StandbyVerticalMode)
                                {
                                        Command newCommand;
                                        newCommand.name = "AFCS-ArmVerticalMode";
                                        newCommand.ivalue = static_cast<int>(Bus::Afcs::VerticalModes::VerticalMode_Off);
                                        bus->commandStream.push_back(newCommand);
                                }
                        }
                        if (bus->AFCS.CurrentVerticalMode == Bus::Afcs::VerticalModes::VerticalSpeed)
                                bus->AFCS.DesiredVsi = 0.0f;

                        if (bus->AFCS.CurrentVerticalMode && bus->AFCS.CurrentVerticalMode != Bus::Afcs::VerticalModes::Altitude)
                                bus->AFCS.StandbyVerticalMode = Bus::Afcs::VerticalModes::Altitude;
                        else
                                bus->AFCS.StandbyVerticalMode = Bus::Afcs::VerticalModes::VerticalMode_Off;
                        bus->commandStream.erase(bus->commandStream.begin() + i);
                }
                else if (command.name == "AFCS-PitchSetting")
                {
                        bus->AFCS.DesiredPitch = command.fvalue;
                        bus->commandStream.erase(bus->commandStream.begin() + i);
                }
                else if (command.name == "AFCS-VsiSetting")
                {
                        bus->AFCS.DesiredVsi = command.fvalue;
                        bus->commandStream.erase(bus->commandStream.begin() + i);
                }
                else if (command.name == "AFCS-AltitudeSetting")
                {
                        bus->AFCS.DesiredAltitude = command.fvalue;
                        bus->commandStream.erase(bus->commandStream.begin() + i);
                }
                else if (command.name == "AFCS-ArmLateralMode")
                {
                        bus->AFCS.StandbyLateralMode = static_cast<Bus::Afcs::LateralModes>(command.ivalue);
                        bus->commandStream.erase(bus->commandStream.begin() + i);
                }
                else if (command.name == "AFCS-ArmVerticalMode")
                {
                        bus->AFCS.StandbyVerticalMode = static_cast<Bus::Afcs::VerticalModes>(command.ivalue);
                        bus->commandStream.erase(bus->commandStream.begin() + i);
                }
                else if (command.name == "AFCS-LateralMode")
                {
                        bus->AFCS.CurrentLateralMode = static_cast<Bus::Afcs::LateralModes>(command.ivalue);

                        if (bus->AFCS.CurrentLateralMode == Bus::Afcs::LateralModes::Roll)
                        {
                                if (fabsf(D3DXToDegree(bus->RollAttitude)) <= 6.0f)
                                        bus->AFCS.DesiredRoll = 0.0f;
                                else if (D3DXToDegree(bus->RollAttitude) >= 35.0f)
                                        bus->AFCS.DesiredRoll = 35.0f;
                                else if (D3DXToDegree(bus->RollAttitude) <= -35.0f)
                                        bus->AFCS.DesiredRoll = -35.0f;
                                else
                                        bus->AFCS.DesiredRoll = bus->RollAttitude;
                        }
                        else if (bus->AFCS.CurrentLateralMode == Bus::Afcs::LateralModes::Heading)
                        {
                                bus->AFCS.DesiredHeading = bus->HeadingTrue;
                        }
                        else if (bus->AFCS.CurrentLateralMode == Bus::Afcs::LateralModes::Navigation)
                        {
                                bus->AFCS.DesiredCourse = -999.0f; // direct
                        }

                        if (bus->AFCS.CurrentLateralMode && bus->AFCS.CurrentVerticalMode == Bus::Afcs::VerticalModes::VerticalMode_Off)
                        {
                                Command newCommand;
                                newCommand.name = "AFCS-VerticalMode";
                                newCommand.ivalue = static_cast<int>(Bus::Afcs::VerticalModes::Pitch);
                                bus->commandStream.push_back(newCommand);
                        }

                        if (!bus->AFCS.CurrentLateralMode)
                        {
                                if (bus->AFCS.CurrentVerticalMode)
                                {
                                        Command newCommand;
                                        newCommand.name = "AFCS-VerticalMode";
                                        newCommand.ivalue = static_cast<int>(Bus::Afcs::VerticalModes::VerticalMode_Off);
                                        bus->commandStream.push_back(newCommand);
                                }
                                if (bus->AFCS.AutopilotEngaged)
                                {
                                        Command newCommand;
                                        newCommand.name = "AFCS-Autopilot";
                                        newCommand.bvalue = false;
                                        bus->commandStream.push_back(newCommand);
                                }
                                if (bus->AFCS.StandbyLateralMode)
                                {
                                        Command newCommand;
                                        newCommand.name = "AFCS-ArmLateralMode";
                                        newCommand.ivalue = static_cast<int>(Bus::Afcs::LateralModes::LateralMode_Off);
                                        bus->commandStream.push_back(newCommand);
                                }
                        }

                        bus->commandStream.erase(bus->commandStream.begin() + i);
                }
                else if (command.name == "AFCS-RollSetting")
                {
                        bus->AFCS.DesiredRoll = command.fvalue;
                        bus->commandStream.erase(bus->commandStream.begin() + i);
                }
                else if (command.name == "AFCS-HeadingRelative")
                {
                        bus->AFCS.DesiredHeading = command.fvalue + bus->HeadingTrue;
                        bus->commandStream.erase(bus->commandStream.begin() + i);
                }
                else if (command.name == "AFCS-AutopilotCycle")
                {
                        Command newCommand;
                        newCommand.name = "AFCS-Autopilot";
                        newCommand.bvalue = !bus->AFCS.AutopilotEngaged;
                        bus->commandStream.push_back(newCommand);
                        bus->commandStream.erase(bus->commandStream.begin() + i);
                }
                else if (command.name == "AFCS-Autopilot")
                {
                        bus->AFCS.AutopilotEngaged = command.bvalue;
                        if (bus->AFCS.AutopilotEngaged)
                        {
                                if (bus->AFCS.CurrentVerticalMode == Bus::Afcs::VerticalModes::VerticalMode_Off)
                                {
                                        Command newCommand;
                                        newCommand.name = "AFCS-VerticalMode";
                                        newCommand.ivalue = static_cast<int>(Bus::Afcs::VerticalModes::Pitch);
                                        bus->commandStream.push_back(newCommand);
                                }
                        }
                        else
                        {
                                Command newCommand;
                                newCommand.name = "APDisconnect";
                                bus->commandStream.push_back(newCommand);
                                if (bus->AFCS.AutothrottleEngaged)
                                {
                                        newCommand.name = "AFCS-Autothrottle";
                                        newCommand.bvalue = false;
                                        bus->commandStream.push_back(newCommand);
                                }
                        }
                        bus->commandStream.erase(bus->commandStream.begin() + i);
                }
                else if (command.name == "AFCS-Autothrottle")
                {
                        if (command.bvalue)
                        {
                                if (bus->AFCS.AutopilotEngaged)
                                        bus->AFCS.AutothrottleEngaged = command.bvalue;
                        }
                        else
                                bus->AFCS.AutothrottleEngaged = command.bvalue;
                        bus->commandStream.erase(bus->commandStream.begin() + i);
                }
                else if (command.name == "AFCS-SetThrust")
                {
                        bus->AFCS.DesiredThrust = command.fvalue;
                        bus->commandStream.erase(bus->commandStream.begin() + i);
                }
        }

#pragma endregion

#pragma region Vertical Modes

        if (bus->AFCS.CurrentVerticalMode == Bus::Afcs::VerticalModes::GlideSlope && bus->waypoints.size() > 0) // G/S
        {
                D3DXVECTOR3 diff3 = bus->Location - bus->waypoints.at(0).absoluteLocation();
                bus->RadioAltitude = -diff3.z;

                D3DXVECTOR2 diff2 = D3DXVECTOR2(diff3.x, diff3.y);
                float dist = D3DXVec2Length(&diff2);
                float desiredaltitude = -(bus->waypoints.at(0).absoluteLocation().z - sinf(0.05235987755982988730771072305466f) * dist); // sin of 0.05 is 3 degree slope

                // positive means we are above
                bus->GlideslopeDeviation = atanf((bus->PressureAltitude - desiredaltitude) / dist);

                // FPA should be -.0523 so we take difference and add it to the current pitch attitude
                float pitchdiff = D3DXToRadian(-3.0f) - bus->FlightPathAngle; // was -.06 .. we want -.0523 so that means we need to pitch up +.01
                float desiredpitch = bus->PitchAttitude + pitchdiff * 0.5f; // 1 deg of pitch doesn't equal 1 deg of fpa?
                desiredpitch -= bus->GlideslopeDeviation * 0.5f;

                // smooth the new pitch down
                bus->AFCS.DesiredPitch += (desiredpitch - bus->AFCS.DesiredPitch) * fElapsedTime;
        }
        else if (bus->AFCS.StandbyVerticalMode == Bus::Afcs::VerticalModes::GlideSlope && bus->waypoints.size() > 1)
        {
                D3DXVECTOR3 diff3 = bus->Location - bus->waypoints.at(1).absoluteLocation();
                bus->RadioAltitude = -diff3.z;

                D3DXVECTOR2 diff2 = D3DXVECTOR2(diff3.x, diff3.y);
                float dist = D3DXVec2Length(&diff2);
                float desiredaltitude = -(bus->waypoints.at(1).absoluteLocation().z - sinf(0.05235987755982988730771072305466f) * dist); // sin of 0.05 is 3 degree slope

                // positive means we are above
                bus->GlideslopeDeviation = atanf((bus->PressureAltitude - desiredaltitude) / dist);
        }
        else
        {
                bus->GlideslopeDeviation = 0.0f;
                bus->RadioAltitude = 0.0f;
        }

        if (bus->AFCS.CurrentVerticalMode == Bus::Afcs::VerticalModes::Altitude) // msl or G/S
        {
                bus->AFCS.DesiredVsi = bus->AFCS.DesiredAltitude - bus->PressureAltitude;
                if ((fabsf(oldDesiredVsi) > 0.3048f && fabsf(bus->AFCS.DesiredVsi) <= 0.3048f) || // roughly 1000 feet
                        (fabsf(oldDesiredVsi) < 0.06096f && fabsf(bus->AFCS.DesiredVsi) >= 0.06096f)) // roughly 200 feet
                {
                        Command newCommand;
                        newCommand.name = "CChord";
                        bus->commandStream.push_back(newCommand);
                }
                oldDesiredVsi = bus->AFCS.DesiredVsi;

                bus->AFCS.DesiredVsi *= 0.03477690288713910761154855643045f; // 1/.3048*.01016
                //if (bus->AFCS.CurrentVerticalMode == Bus::Afcs::VerticalModes::GlideSlope)
                //      bus->AFCS.DesiredVsi *= 2.5f;
                // if we leave the altitude once we got there? 200 feet is the rule in GA, and within 1000 feet of selected
                // so T-1 with 2 is .1 but E-10 with 3.5 is .05
                // limit +/- 6000 fpm (30.5 m/s)
                if (bus->AFCS.DesiredVsi > 0.0305f)
                        bus->AFCS.DesiredVsi = 0.0305f;
                else if (bus->AFCS.DesiredVsi < -0.0305f)
                        bus->AFCS.DesiredVsi = -0.0305f;
                //      desiredvsi*=0.012695510867357302457850903920374;
                //              sprintf_s(msg, 99, "\t  MSL\tcur %.3f\n", f_MSL);
                //              _write(logfile, msg, strlen(msg));
        }
        /*
        if (verticalmode == 3) // ias
        {
        // 0.090 - 0.080 is +0.010 is 1 deg so *100
        float deltaias = -(desiredias - ias);
        float targetpitch = horizonpitch + deltaias*3.0f; // was 10
        float diff = targetpitch - desiredpitch;
        if (diff>0.052359877559829887307710723054658f) // 3 degrees per second
        diff = 0.052359877559829887307710723054658f;
        else if (diff<-0.052359877559829887307710723054658f)
        diff = -0.052359877559829887307710723054658f;
        desiredpitch += diff*fElapsedTime; // 0.1-0.01=0.09
        // deg limit
        if (desiredpitch>0.34906585039886591538473815369772f) // nose up 20
        desiredpitch = 0.34906585039886591538473815369772f;
        else if (desiredpitch<-0.26179938779914943653855361527329f) // nose down 15
        desiredpitch = -0.26179938779914943653855361527329f;
        //              sprintf_s(msg, 99, "\t  VSI\tcur %.3f tgt %.3f\n", vsi, desiredvsi);
        //              _write(logfile, msg, strlen(msg));
        }
        */

        // v/s, higher modes than IAS, T/O (T/O used to be a vertical speed mode!? yup, +/-0.1 (km/s?) )
        if (bus->AFCS.CurrentVerticalMode == Bus::Afcs::VerticalModes::VerticalSpeed ||
                bus->AFCS.CurrentVerticalMode == Bus::Afcs::VerticalModes::Altitude ||
                bus->AFCS.CurrentVerticalMode == Bus::Afcs::VerticalModes::SpeedPitch)
        {
                static float smoothalt = 0.0f;
                smoothalt += (bus->PressureAltitude - smoothalt) * fElapsedTime * 2.0f; // was 0.25f no elapsed

                static float lastAltitudeUpdate = 0.0f;
                lastAltitudeUpdate += fElapsedTime;
                static float oldaltitude = 0.0f;
                float altdiff = smoothalt - oldaltitude;
                if (altdiff != 0.0f)
                {
                        bus->VerticalSpeed = altdiff / fElapsedTime;
                        lastAltitudeUpdate = 0.0f;
                }
                else if (lastAltitudeUpdate > 1.0f)
                        bus->VerticalSpeed = 0.0f;
                oldaltitude = smoothalt;


                // 0.020 - 0.010 is +0.010 is 1 deg so *100
                float deltavsi = bus->AFCS.DesiredVsi - bus->VerticalSpeed;
                float targetpitch = bus->PitchAttitude + deltavsi * 3.0f;
                float diff = targetpitch - bus->AFCS.DesiredPitch;
                if (diff > 0.052359877559829887307710723054658f) // 3 degrees per second
                        diff = 0.052359877559829887307710723054658f;
                else if (diff < -0.052359877559829887307710723054658f)
                        diff = -0.052359877559829887307710723054658f;
                //if (bus->AFCS.CurrentVerticalMode == Bus::Afcs::VerticalModes::GlideSlope)
                //{
                //      if (diff > 0.025f) // 1.5 degrees per second
                //              diff = 0.025f;
                //      else if (diff<-0.025f)
                //              diff = -0.025f;
                //}
                bus->AFCS.DesiredPitch += diff * fElapsedTime; // 0.1-0.01=0.09
                // deg limit
                if (bus->AFCS.DesiredPitch > 0.34906585039886591538473815369772f) // nose up 20
                        bus->AFCS.DesiredPitch = 0.34906585039886591538473815369772f;
                else if (bus->AFCS.DesiredPitch < -0.26179938779914943653855361527329f) // nose down 15
                        bus->AFCS.DesiredPitch = -0.26179938779914943653855361527329f;
                //              sprintf_s(msg, 99, "\t  VSI\tcur %.3f tgt %.3f\n", vsi, desiredvsi);
                //              _write(logfile, msg, strlen(msg));
        }


        if (bus->AFCS.CurrentVerticalMode && bus->AFCS.AutopilotEngaged) // pitch mode
        {
                // smooth pitch
                static float pitch = 0.0f;
                pitch += (bus->PitchAttitude - pitch) * fElapsedTime * 2.0f; // was 0.25f no elapsed

                static float oldpitchrate = 0.0f;

                float deltapitch;
                if (fabsf(bus->RollAttitude) < D3DX_PI) // unusual attitudes
                        deltapitch = bus->AFCS.DesiredPitch - pitch;//-playerships[ourcockpit.ourshipC].pitch;
                else
                        deltapitch = -bus->AFCS.DesiredPitch - pitch;//-playerships[ourcockpit.ourshipC].pitch;

                float currentrate = (pitch - oldPitchAttitude) / fElapsedTime;

                // 10 deg, we do max of 3 deg per sec
                float desiredrate = deltapitch * 0.3f;
                // never more than 3 deg per sec
                if (desiredrate > 0.052359877559829887307710723054658f)
                        desiredrate = 0.052359877559829887307710723054658f;
                else if (desiredrate < -0.052359877559829887307710723054658f)
                        desiredrate = -0.052359877559829887307710723054658f;


                //              float authority=fabsf(playerships[ourcockpit.ourshipC].pitch/0.052359877559829887307710723054658f);
                //              if (authority>10.0f) // 3 deg/sec==1.0f
                //                      authority=10.0f;
                //              else if (authority<0.1f)
                //                      authority=0.1f;
                //              desiredrate*=1.0f/authority;

                static float oldcorrection = 0.0f;
                // we want to effect a change on the pitch of a certain number of deg/s/s so we look at how much it did
                // from one frame to another and magnify the effect
                float effectivechange = oldpitchrate - currentrate;
                float magnify = 1.0f;
                if (oldcorrection != 0.0f && bus->IndicatedAirspeed > Vs1)
                        magnify = fabsf(oldcorrection - effectivechange) / fabsf(oldcorrection) + 1.0f;
                //              magnify*=magnify; // 4
                if (magnify > 10.0f)
                        magnify = 10.0f;
                else if (magnify < 0.1f)
                        magnify = 0.1f;

                //desiredrate *= magnify;
                // follow a running magnify average
                //              static float magavg=200.0f;
                //              magavg*=0.995f; // was 100, now 99
                //              magavg+=magnify; // now 10 again


                //              char msg[999];
                //sprintf_s( msg, 999, "AF* current %.1f  requested %.1f  magnify %.1f  desiredrate %.1f/s\n",
                //D3DXToDegree(horizonpitch), D3DXToDegree(desiredpitch), magnify, D3DXToDegree(desiredrate) );
                //_write(logfile, msg, strlen(msg));

                oldpitchrate = currentrate;
                oldPitchAttitude = pitch; // save for next time where it should have been

                //              oldcorrection=(desiredrate-playerships[ourcockpit.ourshipC].pitch)*fElapsedTime*ourcockpit.appitagr;
                oldcorrection = (desiredrate - currentrate) * magnify * fElapsedTime * 0.3f * PitchAggression;
                if (oldcorrection > 0.52359877559829887307710723054658f)
                        oldcorrection = 0.52359877559829887307710723054658f;
                else if (oldcorrection < -0.52359877559829887307710723054658f)
                        oldcorrection = -0.52359877559829887307710723054658f;
                //oldcorrection *= fElapsedTime;

                bus->AFCS.PitchInput = Library::MathUtils::Clamp(bus->AFCS.PitchInput + oldcorrection, -1.0f, 1.0f);

                float transfer = bus->PitchControl * 0.02f * fElapsedTime; // will be there in less than 60 sec times pitch agressivness
                if (bus->PitchTrim > 0.2618f) // 15°
                        bus->PitchTrim = 0.2618f;
                else if (bus->PitchTrim < -0.2618f)
                        bus->PitchTrim = -0.2618f;
                else
                        bus->PitchTrim += transfer;

                if (fabsf(bus->AFCS.PitchInput + bus->PitchTrim) >= 0.5f)
                {
                        Command command;
                        command.name = "AltitudeCoupler";
                        bus->commandStream.push_back(command);
                }

                // we want pitch to be 0 when desiredpitch==horizonpitch, 3deg/sec when desiredpitch is 10 off
                //              sprintf_s(msg, 99, "\tPITCH\ttgt %.3f - cur %.3f = dlt %.3f (sat rat %.3f) - cur %.3f = trm %.3f\n", D3DXToDegree(desiredpitch), D3DXToDegree(horizonpitch), D3DXToDegree(deltapitch), desiredrate, playerships[ourcockpit.ourshipC].pitch, trim);
                //              _write(logfile, msg, strlen(msg));
        }

#pragma endregion

        // rules like we are squat switched and not TO/TO but have active mode then switch to TO/TO

#pragma region Lateral Modes

        if (bus->AFCS.CurrentLateralMode == Bus::Afcs::LateralModes::Navigation ||
                bus->AFCS.CurrentLateralMode == Bus::Afcs::LateralModes::Localizer) // nav mode/loc mode determines a heading
        {
                // if we are in loc/nav and we run out of waypoints then we go to heading mode?
                if (bus->waypoints.size() > 0)
                {
                        Waypoint nextWaypoint = bus->waypoints.at(0);
                        D3DXVECTOR3 result = bus->Location - nextWaypoint.absoluteLocation();
                        D3DXVECTOR2 result2 = D3DXVECTOR2(result.x, result.y);
                        float dist = D3DXVec2Length(&result2);
                        float bear = atan2f(result2.y, result2.x) - D3DX_PI * 0.5f - bus->HeadingTrue;
                        if (bear >= D3DX_PI * 2.0f)
                                bear -= D3DX_PI * 2.0f;
                        else if (bear < 0.0f)
                                bear += D3DX_PI * 2.0f;

                        bus->HeadingTrack = bus->HeadingTrue; // no wind


                        //LOOK HERE, we were 352.7 he was bear 17.3 so course is 10

                        // start with desiredcrs given
                        // heading to object
                        float hdg2obj = bus->HeadingTrue + bear; // 020+020 is 040
                        if (hdg2obj > D3DX_PI * 2.0f)
                                hdg2obj -= D3DX_PI * 2.0f;
                        else if (hdg2obj < 0.0f)
                                hdg2obj += D3DX_PI * 2.0f;


                        if (bus->AFCS.DesiredCourse == -999.0f) // direct
                                bus->AFCS.DesiredCourse = hdg2obj;


                        // difference between them should be 0
                        // this is where we need to look for flyover vs. flyby
                        // http://rise.unistellar.com/jira/view.php?id=456
                        float correct = hdg2obj - bus->AFCS.DesiredCourse; // 020-350 is -330
                        if (correct > D3DX_PI) // 359
                                correct -= D3DX_PI * 2.0f; // to fix case of 359-0 is 359 instead of -1
                        else if (correct < -D3DX_PI)
                                correct += D3DX_PI * 2.0f;
                        if (correct > D3DX_PI * 0.5f) // 179
                                correct = D3DX_PI - correct; // 1 to the right
                        else if (correct < -D3DX_PI * 0.5f) // -179
                                correct = -D3DX_PI - correct;

                        bus->LocalizerDeviation = correct;

                        if (bus->AFCS.CurrentLateralMode == Bus::Afcs::LateralModes::Localizer)
                                correct *= 4.0f;
                        else
                                correct *= 2.0f; // overcorrect to intercept... 1.0f would just be parallel
                        if (correct > D3DX_PI * 0.25f) // no more than 45 degrees though
                                correct = D3DX_PI * 0.25f;
                        else if (correct < -D3DX_PI * 0.25f)
                                correct = -D3DX_PI * 0.25f;
                        if (dist < 4.0f) // and reduce correction based on distance
                                correct *= dist * 0.25f;

                        // if insttrk is 320 and we are 330 then we need to pretend we are +10
                        float trkadj = bus->HeadingTrue - bus->HeadingTrack; // +10
                        if (trkadj > D3DX_PI * 2.0f)
                                trkadj -= D3DX_PI * 2.0f;
                        else if (trkadj < 0.0f)
                                trkadj += D3DX_PI * 2.0f;

                        bus->AFCS.DesiredHeading = bus->AFCS.DesiredCourse + correct + trkadj;
                        if (bus->AFCS.DesiredHeading > D3DX_PI * 2.0f)
                                bus->AFCS.DesiredHeading -= D3DX_PI * 2.0f;
                        else if (bus->AFCS.DesiredHeading < 0.0f)
                                bus->AFCS.DesiredHeading += D3DX_PI * 2.0f;
                }
                else 
                {
                        Command newCommand;
                        newCommand.name = "AFCS-LateralMode";
                        newCommand.ivalue = static_cast<int>(Bus::Afcs::LateralModes::Heading);
                        bus->commandStream.push_back(newCommand);
                }
        }


        if (bus->AFCS.CurrentLateralMode >= Bus::Afcs::LateralModes::Heading) // hdg mode
        {
                float desiredroll = bus->AFCS.DesiredHeading - bus->HeadingTrue; // we were 030 and selected 360, that is 330 greater than 180

                if (desiredroll < -D3DX_PI)
                        desiredroll = D3DX_PI * 2.0f + bus->AFCS.DesiredRoll;
                else if (desiredroll > D3DX_PI) // more than 180 to right
                        desiredroll = bus->AFCS.DesiredRoll - D3DX_PI * 2.0f;
                //desiredroll *= 0.333333f*3.0f; // agressive A-4 is 1.0f // 30 bank is 10 deg

                // limit to 30 deg of bank
                if (desiredroll > 0.523599f)
                        desiredroll = 0.523599f;
                else if (desiredroll < -0.523599f)
                        desiredroll = -0.523599f;

                // smooth the new roll
                bus->AFCS.DesiredRoll += (desiredroll - bus->AFCS.DesiredRoll) * fElapsedTime;
        }


        if (bus->AFCS.CurrentLateralMode && bus->AFCS.AutopilotEngaged) // roll mode
        {
                // we want 30 deg bank, we are at 0 deg bank so that is 30deg
                float deltaroll = bus->AFCS.DesiredRoll - bus->RollAttitude;//-D3DXToDegree(playerships[ourcockpit.ourshipC].roll); // degrees, can be up to 60 usually 30

                // desiredrate is 10deg/s
                float desiredrate = deltaroll * 0.3f; // 10 deg/s for 30 deg
                //              desiredrate=D3DXToRadian(desiredrate); everything is in rads
                // we want 10 deg per sec max
                desiredrate = Library::MathUtils::Clamp(desiredrate, -0.1745329252f, 0.1745329252f);
                // we want roll to be 0 when desiredroll==horizonroll, 10deg/sec when desiredpitch is 30 off
                // so we want to increase/decrease the rate

                float currentRollRate = (bus->RollAttitude - oldRollAttitude) / fElapsedTime; // rad per second
                oldRollAttitude = bus->RollAttitude;

                float ratediff = desiredrate - currentRollRate; // we want 10deg per sec, we have 0deg per sec
                // so ratediff=10deg/sec, 0 deg/sec is centered, 10deg/sec is full right

                float oldcorrection = (ratediff * 3.0f - bus->AFCS.RollInput) * RollAggression * 0.5235987756f; // agressiveness, A-4 is 1.0f
                //oldcorrection = Clamp(oldcorrection, -0.523599f, 0.523599f); // limit to 30 deg of bank? I commented this out 4/18/2015 

                bus->AFCS.RollInput = Library::MathUtils::Clamp(bus->AFCS.RollInput + oldcorrection * fElapsedTime, -1.0f, 1.0f);
                //              char msg[999];
                //sprintf_s(msg, "desired %.1f° - horizon %.1f° = delta %.1f° *0.3= ratechange %frad - currate %frad = ratediff %frad >>> rolladj %frad/s\n",
                //desiredroll, horizonroll, deltaroll, desiredrate, playerships[ourcockpit.ourshipC].roll, ratediff, ratediff*3.0f-roll);
                //_write(logfile, msg, strlen(msg));


                //              sprintf_s(msg, 99, "\tROLL\ttgt %.3f - cur %.3f = dlt %.3f (rad %.3f - cur %.3f = trm %.3f)\n",
                //                      D3DXToDegree(desiredroll), D3DXToDegree(horizonroll), D3DXToDegree(deltaroll), desiredrate, playerships[ourcockpit.ourshipC].roll, roll);
                //              _write(logfile, msg, strlen(msg));
        }

#pragma endregion
}