FMS.cpp

Go to the documentation of this file.

#include "Module.h"

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

void Fms::FrameMove(float fElapsedTime)
{
        if (bus->waypoints.size() == 0)
                return;

        static float oldbear = 0.0f;

        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; // so this would give us 0 is north... if we are facing west, as an example, it would be 360 - 270 = +90 
        while (bear >= D3DX_PI) bear -= D3DX_PI * 2.0f;
        while (bear <= -D3DX_PI) bear += D3DX_PI * 2.0f;

        // right now we are doing "fly-over"
        if (fabsf(oldbear) < D3DX_PI * 0.5f && fabsf(bear) > D3DX_PI * 0.5f) // was less than 90 left or right
        {
                if (dist < 0.5f && nextWaypoint.type == Waypoint::WaypointType::FinalApproachFix) // .25km laterally
                {
                        if (bus->AFCS.StandbyLateralMode == Bus::Afcs::LateralModes::Localizer)
                        {
                                Command newCommand;
                                newCommand.name = "AFCS-LateralMode";
                                newCommand.ivalue = static_cast<int>(Bus::Afcs::LateralModes::Localizer);
                                bus->commandStream.push_back(newCommand);
                                newCommand.name = "AFCS-ArmLateralMode";
                                newCommand.ivalue = static_cast<int>(Bus::Afcs::LateralModes::LateralMode_Off);
                                bus->commandStream.push_back(newCommand);
                        }
                        if (bus->AFCS.StandbyVerticalMode == Bus::Afcs::VerticalModes::GlideSlope)
                        {
                                Command newCommand;
                                newCommand.name = "AFCS-VerticalMode";
                                newCommand.ivalue = static_cast<int>(Bus::Afcs::VerticalModes::GlideSlope);
                                bus->commandStream.push_back(newCommand);
                                newCommand.name = "AFCS-ArmVerticalMode";
                                newCommand.ivalue = static_cast<int>(Bus::Afcs::VerticalModes::VerticalMode_Off);
                                bus->commandStream.push_back(newCommand);
                        }
                }
                bus->waypoints.erase(bus->waypoints.begin());
                bus->AFCS.DesiredCourse = D3DX_PI * 1.5f; 
        }

        oldbear = bear;
}