Xplane.cpp

Go to the documentation of this file.

#include "Xplane.h"

namespace Devices
{
        void Xplane::Initialize(Logger* prmLogger, XplaneConfig* prmConfig, Bus* prmBus)
        {
                logger = prmLogger;
                config = prmConfig;
                bus = prmBus;

                if (!config->enabled) return;

                WSADATA data;
                if (WSAStartup(MAKEWORD(2, 2), &data) != 0)
                {
                        logger->Log("Xplane::Initialize WSAStartup failed!", Logger::Error);
                        config->enabled = false;
                        return;
                }

                fd = socket(AF_INET, SOCK_DGRAM, IPPROTO_UDP);
                if (fd == SOCKET_ERROR)
                {
                        char msg[99];
                        sprintf_s(msg, 99, "Xplane::Initialize cannot create socket (error %i)", WSAGetLastError());
                        logger->Log(msg, Logger::Error);
                        config->enabled = false;
                        return;
                }

                struct sockaddr_in myaddr;
                memset(reinterpret_cast<char *>(&myaddr), 0, sizeof(myaddr));
                myaddr.sin_family = AF_INET ;
                myaddr.sin_addr.s_addr = htonl(INADDR_ANY);
                myaddr.sin_port = htons(config->port);
                if (bind(fd, reinterpret_cast<struct sockaddr *>(&myaddr), sizeof(myaddr)) == -1)
                {
                        logger->Log("Xplane::Initialize bind failed!", Logger::Error, WSAGetLastError());
                        config->enabled = false;
                        return;
                }
        }

        void Xplane::FrameMove() const
        {
                unsigned long bytesWaiting = 0;
                if (ioctlsocket(fd, FIONREAD, &bytesWaiting))
                {
                        logger->Log("Xplane::FrameMove ioctlsocket failed!", Logger::Error, WSAGetLastError());
                        config->enabled = false;
                        return;
                }

                if (bytesWaiting == 0) return;

                char buf[199];

                int recvlen = recvfrom(fd, buf, sizeof(buf), 0, nullptr, nullptr);
                if (recvlen > 5)
                {
                        if (buf[0] == 'D' && buf[1] == 'A' && buf[2] == 'T' && buf[3] == 'A' && buf[4] == '@')
                        {
                                int b = 5;
                                int bytesLeft = recvlen - b;
                                int sentences = bytesLeft / 36;
                                for (int i = 0; i < sentences; i++)
                                {
                                        sentence sent;
                                        memcpy(&sent, &buf[b], sizeof(sent));
                                        switch (sent.group)
                                        {
                                        case 3:
                                                // group 3, 0 is _Vind,_kias, 1 is _Vind,_keas, 2 is Vtrue,_ktas, 3 is Vtrue,_ktgs, 4 is unused, 5 is _Vind,__mph, 6 is Vtrue,mphas, 7 is Vtrue,mphgs
                                                bus->IndicatedAirspeed = sent.data[0].floatData * 0.514444f; // nautical per hour to meters per second
                                                break;
                                        case 4:
                                                // 0 is _Mach,ratio, 1 is unused, 2 is _WI,__fpm, 3 is unused, 4 is Gload,norml, 5 is Gload,axial, 6 is Gload,_side, 7 is unused
                                                // norml = updown (positive is down/gravity), axial = frontback (positive is back), side = leftright (positive is left)
                                                bus->NormalAcceleration = D3DXVECTOR3(sent.data[6].floatData, sent.data[5].floatData, sent.data[4].floatData);
                                                break;
                                        case 8:
                                                // 0 is elev, 1 is aileron, 2 is rudder, 3 is unused, 4 is unused, 5 is unused, 6 is unused, 7 is unused
                                                bus->PitchControl = sent.data[0].floatData;
                                                bus->RollControl = sent.data[1].floatData;
                                                bus->YawControl = sent.data[2].floatData;
                                                break;
                                        case 17:
                                                // group 17, 0 is pitch, 1 is roll, 2 is true heading, 3 is magnetic heading, 4 is unused, 5 is unused, 6 is unused, 7 is unused
                                                bus->PitchAttitude = sent.data[0].floatData;
                                                bus->RollAttitude = sent.data[1].floatData;
                                                bus->HeadingTrue = sent.data[2].floatData;
                                                //bus->HeadingMagnetic = sent.data[3].floatData;
                                                break;
                                        case 20:
                                                // group 20, 0 is latitude, 1 is longitude, 2 is MSL altitude, 3 is AGL altitude, 4 is squat switch, 5 is indicated altitude (baro adjusted?), 6 is rounded latitude?, 7 is rounded longitude?
                                                bus->Latitude = sent.data[0].floatData;
                                                bus->Longitude = sent.data[1].floatData;
                                                bus->PressureAltitude = sent.data[2].floatData;
                                                bus->RadioAltitude = sent.data[3].floatData;
                                                bus->OnGround = sent.data[4].floatData == 1.0f;
                                                break;
                                        }
                                        b += 36;
                                }
                                //printf("\n");
                        }
                        else
                        {
                                char msg[199];
                                sprintf_s(msg, 199, "Xplane::FrameMove Received %i bytes NOT DATA header", recvlen);
                                logger->Log(msg, Logger::Warn);
                        }
                }
                else
                {
                        char msg[199];
                        sprintf_s(msg, 199, "Xplane::FrameMove Received %i bytes NOT DATA header", recvlen);
                        logger->Log(msg, Logger::Warn);
                }
        }

        Xplane::~Xplane()
        {
                if (!config || !config->enabled) return;

                WSACleanup();
        }
}