FCS.cpp

Go to the documentation of this file.

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

void Fcs::FrameMove(float fElapsed)
{
        if (*fcsGetFloatPtr <= 48.0f)
                return;

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

                int location = command.ivalue;

                if (command.name == "FlightControlPriorityDepress")
                {
                        if (locations.size() > static_cast<unsigned>(location))
                        {
                                ControlPriorityLocation = location;
                                Command newCommand;
                                newCommand.name = location == 0 ? "FCSPriorityRight" : "FCSPriorityLeft";
                                bus->commandStream.push_back(newCommand);
                                locations.at(location).priorityButtonDown = true;
                                locations.at(location).priorityHeldDownSec = 0.0f;
                                ControlPriorityLatched = false;
                        }
                        bus->commandStream.erase(bus->commandStream.begin() + i);
                }
                else if (command.name == "FlightControlPriorityRelease")
                {
                        if (locations.size() > static_cast<unsigned>(location))
                        {
                                locations.at(location).priorityButtonDown = false;
                                if (ControlPriorityLocation == location && !ControlPriorityLatched) // other pilot may have pressed it after this guy
                                        ControlPriorityLocation = -1;
                        }
                        bus->commandStream.erase(bus->commandStream.begin() + i);
                }
                else if (command.name == "PushToTalkDepress")
                {
                        if (locations.size() > static_cast<unsigned>(location))
                        {
                                Command newCommand;
                                newCommand.name = location == 0 ? "Push To Talk Pilot" : "Push To Talk Gunner";
                                newCommand.ivalue = 1;
                                bus->commandStream.push_back(newCommand);
                                bus->Transmitting.at(location) = true;
                        }
                        bus->commandStream.erase(bus->commandStream.begin() + i);
                }
                else if (command.name == "PushToTalkRelease")
                {
                        if (locations.size() > static_cast<unsigned>(location))
                        {
                                Command newCommand;
                                newCommand.name = location == 0 ? "Push To Talk Pilot" : "Push To Talk Gunner";
                                newCommand.ivalue = 0;
                                bus->commandStream.push_back(newCommand);
                                bus->Transmitting.at(location) = false;
                        }
                        bus->commandStream.erase(bus->commandStream.begin() + i);
                }
        }
#pragma endregion

        if (locations.size() > 1)
        {
                for (UINT i = 0; i < locations.size(); i++)
                {
                        if (locations.at(i).priorityButtonDown)
                        {
                                locations.at(i).priorityHeldDownSec += fElapsed;
                                if (locations.at(i).priorityHeldDownSec >= 40.0f && !ControlPriorityLatched)
                                {
                                        ControlPriorityLocation = i;
                                        ControlPriorityLatched = true;
                                }
                        }
                }
        }

        bool dualInput = false;
        if (ControlPriorityLocation == -1)
        {
                bus->PitchControl = 0.0f;
                bus->RollControl = 0.0f;
                bus->YawControl = 0.0f;
                if (bus->AFCS.AutopilotEngaged)
                {
                        bus->PitchControl += bus->AFCS.PitchInput;
                        bus->RollControl += bus->AFCS.RollInput;
                        bus->YawControl += bus->AFCS.YawInput;
                }
                for (UINT i = 0; i < flightControls.size(); i++)
                {
                        if (flightControls.at(i)->IsActive())
                        {
                                bus->PitchControl += flightControls.at(i)->GetAxis(1) * bus->occupantPitchControlAuthority;
                                bus->RollControl += flightControls.at(i)->GetAxis(0) * bus->occupantRollControlAuthority;
                                bus->YawControl += flightControls.at(i)->GetAxis(5) * max(bus->occupantPitchControlAuthority, bus->occupantRollControlAuthority);

                                if (i == 0)
                                        *fcsFaultPtr &= ~1;
                                else
                                        *fcsFaultPtr &= ~2;
                        }
                        else
                        {
                                if (i == 0)
                                        *fcsFaultPtr |= 1;
                                else
                                        *fcsFaultPtr |= 2;
                        }
                }

                if (bus->PitchControl > 1.0f)
                {
                        bus->PitchControl = 1.0f;
                        dualInput = true;
                }
                if (bus->RollControl > 1.0f)
                {
                        bus->RollControl = 1.0f;
                        dualInput = true;
                }
                if (bus->YawControl > 1.0f)
                {
                        bus->YawControl = 1.0f;
                        dualInput = true;
                }
                if (bus->PitchControl < -1.0f)
                {
                        bus->PitchControl = -1.0f;
                        dualInput = true;
                }
                if (bus->RollControl < -1.0f)
                {
                        bus->RollControl = -1.0f;
                        dualInput = true;
                }
                if (bus->YawControl < -1.0f)
                {
                        bus->YawControl = -1.0f;
                        dualInput = true;
                }
        }
        else
        {
                if (flightControls.at(ControlPriorityLocation)->IsActive())
                {
                        bus->PitchControl = flightControls.at(ControlPriorityLocation)->GetAxis(1) * bus->occupantPitchControlAuthority;
                        bus->RollControl = flightControls.at(ControlPriorityLocation)->GetAxis(0) * bus->occupantRollControlAuthority;
                        bus->YawControl = flightControls.at(ControlPriorityLocation)->GetAxis(5) * max(bus->occupantPitchControlAuthority, bus->occupantRollControlAuthority);
                }
        }

        if (dualInput && !bus->IsCommandOnStack("FCSDualInput"))
        {
                Command newCommand;
                newCommand.name = "FCSDualInput";
                bus->commandStream.push_back(newCommand);
        }

        for (UINT i = 0; i < flightControls.size(); i++)
        {
                if (flightControls.at(i)->GetButton(6))
                {
                        bus->PitchTrim -= fElapsed * 0.03f;
                        if (bus->PitchTrim < -0.2618f)
                                bus->PitchTrim = -0.2618f;
                }
                if (flightControls.at(i)->GetButton(8))
                {
                        bus->PitchTrim += fElapsed * 0.03f;
                        if (bus->PitchTrim > 0.2618f)
                                bus->PitchTrim = 0.2618f;
                }
        }


        if (true) 
        {
                static float oldhdg = 0.0f;
                float _yaw = (bus->HeadingTrue - oldhdg);
                if (_yaw < -D3DX_PI) _yaw += D3DX_PI * 2.0f;
                if (_yaw > D3DX_PI) _yaw -= D3DX_PI * 2.0f;
                oldhdg = bus->HeadingTrue;
                _yaw /= fElapsed; // radians per second

                float desiredyaw = 0.0f;
                if (bus->IndicatedAirspeed != 0.0f)
                        desiredyaw = 19.041542139258135684237466284211f * tanf(bus->RollAttitude) * cos(bus->PitchAttitude) / (bus->IndicatedAirspeed / sqrtf(bus->AirDensity) * 1943.84449f); // km/s to knots is 1943.84449
                float yawcorr = desiredyaw - _yaw; // say we are 10 and we want 0 then 0-10 is -10 perfect!

                float dampen = yawcorr / 0.05235987756f; // Yaw damper
                dampen = Library::MathUtils::Clamp(dampen, -0.3f, 0.3f);
                bus->YawControl += dampen;
                bus->YawControl = Library::MathUtils::Clamp(bus->YawControl, -1.0f, 1.0f);
        }


        const float Vs = 0.010f;
        const float Vne = 0.128f; // Vne at 60km
        float ifcs = 1.0f - (bus->IndicatedAirspeed - Vs) / (Vne - Vs);
        if (ifcs > 1) ifcs = 1;
        if (ifcs < 0) ifcs = 0;
        ifcs *= ifcs; // velocity squared


        ZeroMemory(bus->DiscreetSurfacePostion, sizeof(bus->DiscreetSurfacePostion));

        const float modarea = 31.796473f / 55.824566f; // lower surface is bigger
        const float modang = 0.739191473f; // the lower surface is less conducive to roll (need to increase) and more conducive to pitch (need to decrease)
        const float modcg = 0.14285714285714285714285714285714f; // since the top controls are significantly above the CG compared to the lower controls below...
        const float moddeflect = 0.392699f / 0.5f; // bottom surfaces can deflect further
        //float r1 = modcg / modarea;
        //float r2 = 1.0f / modang*moddeflect;
        //float r3 = modang*moddeflect;
        const float modusefullrange = 1.0f / 1.06250954f;

        bus->RollControlSurfacePosition = bus->RollControl * ifcs;
        float pitreq = Library::MathUtils::Clamp(-bus->PitchControl - bus->PitchTrim, -1.0f, 1.0f) * ifcs;
        bus->YawControlSurfacePosition = bus->YawControl * ifcs;

        bus->DiscreetSurfacePostion[0] -= bus->RollControlSurfacePosition * modcg / modarea * modusefullrange; // top right from rear
        bus->DiscreetSurfacePostion[1] -= bus->RollControlSurfacePosition / modang * moddeflect * modusefullrange; // lower right from rear
        bus->DiscreetSurfacePostion[2] += bus->RollControlSurfacePosition / modang * moddeflect * modusefullrange; // lower left from rear
        bus->DiscreetSurfacePostion[3] += bus->RollControlSurfacePosition * modcg / modarea * modusefullrange; // top left from rear

        bus->DiscreetSurfacePostion[0] += pitreq * modcg / modarea * modusefullrange;
        bus->DiscreetSurfacePostion[1] += pitreq * modang * moddeflect * modusefullrange;
        bus->DiscreetSurfacePostion[2] += pitreq * modang * moddeflect * modusefullrange;
        bus->DiscreetSurfacePostion[3] += pitreq * modcg / modarea * modusefullrange;

        bus->DiscreetSurfacePostion[0] -= bus->YawControlSurfacePosition * modcg / modarea * modusefullrange;
        bus->DiscreetSurfacePostion[1] += bus->YawControlSurfacePosition * modang * moddeflect * modusefullrange * 0.1f;
        bus->DiscreetSurfacePostion[2] -= bus->YawControlSurfacePosition * modang * moddeflect * modusefullrange * 0.1f;
        bus->DiscreetSurfacePostion[3] += bus->YawControlSurfacePosition * modcg / modarea * modusefullrange;


        float max = max(max(max(fabsf(bus->DiscreetSurfacePostion[0]), fabsf(bus->DiscreetSurfacePostion[1])), fabsf(bus->DiscreetSurfacePostion[2])), fabsf(bus->DiscreetSurfacePostion[3]));
        if (max > 1.0f)
                max = 1.0f / max;
        else
                max = 1.0f;

        bus->DiscreetSurfacePostion[0] *= max;
        bus->DiscreetSurfacePostion[1] *= max;
        bus->DiscreetSurfacePostion[2] *= max;
        bus->DiscreetSurfacePostion[3] *= max;
}

Fcs::Fcs(Bus* prmBus, int joysticks) : Module(prmBus)
{
        locations.resize(joysticks);
        bus->Transmitting.resize(joysticks);
}

void Fcs::Initialize(Devices::Joystick* prmFlightControl)
{
        flightControls.push_back(prmFlightControl);
}

void Fcs::ConnectComponent(std::string type, std::string guid)
{
        fcsGetFloatPtr = bus->GetComponentCurrentStatePtr(guid);
        fcsFaultPtr = reinterpret_cast<DWORD*>(bus->GetComponentFaultStatePtr(guid));
}