AHRSInputIOProcessor.cpp

#include "AHRSInputIOProcessor.hpp"


/*
 * Return Values:
 * 0 : No Errors.
 * 1 : Serial port does not exist.
 * 2 : Some other error occurred.
 * 3 : Error getting state.
 * 4 : Error setting serial port state
 * 5 : Error occured while setting timoemouts.
 * 6 : Error occured while trying to send GoToConfig message.
 * 7 : Error occured while trying to send SetOutputConfiguration message.
 * 8 : Error occured while trying to send GoToMeasurment message.
*/

int AHRSInputIOProcessor::configAHRS()
{
    hSerial = CreateFile("COM8",						// The COM port to connect to, may have to be changed
                          GENERIC_READ | GENERIC_WRITE,	// depending on what COM port gets selected.
                          0,
                          0,
                          OPEN_EXISTING,
                          FILE_ATTRIBUTE_NORMAL,
                          0);
                          
    if (hSerial == INVALID_HANDLE_VALUE)
    {
        if(GetLastError() == ERROR_FILE_NOT_FOUND)
        {
            std::cerr << "Serial port does not exist." << std::endl;
            return 1;
        }
        std::cerr << "Some other error occurred." << std::endl;
        return 2;
    }

    DCB dcb = { 0 };
    dcb.DCBlength = sizeof(dcb);
    
    if (!GetCommState(hSerial, &dcb))
    {
        std::cerr << "Error getting state." << std::endl;
        return 3;
    }
    
    dcb.BaudRate = CBR_115200; // defualt baud rate of MT
    dcb.ByteSize = 8;
    dcb.StopBits = ONESTOPBIT;
    dcb.Parity = NOPARITY;
    
    if (!SetCommState(hSerial, &dcb))
    {
        std::cerr << "Error setting serial port state." << std::endl;
        return 4;
    }
    
    COMMTIMEOUTS timeouts = {0};

    timeouts.ReadIntervalTimeout = 50;
    timeouts.ReadTotalTimeoutConstant = 50;
    timeouts.ReadTotalTimeoutMultiplier = 10;
    timeouts.WriteTotalTimeoutConstant = 50;
    timeouts.WriteTotalTimeoutMultiplier = 10;

    if (!SetCommTimeouts(hSerial, &timeouts))
    {
        std::cerr << "Error occured while setting timoemouts." << std::endl;
        return 5;
    }

    std::cout << "Connection established!" << std::endl;
    std::cout << "Setting up Config..." << std::endl;

    unsigned char wBuff1[] = {0xFA,0xFF,0x30,0x00,0xD1}; // This is the GoToConfig message
    DWORD dwBytesWritten = 0; // Number of bytes actually written

    if (!WriteFile(hSerial, wBuff1, 6, &dwBytesWritten, NULL))
    {
        std::cerr << "Error occured while trying to send GoToConfig message." << std::endl;
        return 6;
    }

    unsigned char wBuff2[] = {0xFA,0xFF,0xC0,0x04,0x20,0x33,0x00,0x00,0xEA};	// This is the SetOutputConfiguration message
    dwBytesWritten = 0;

    if (!WriteFile(hSerial, wBuff2, 10, &dwBytesWritten, NULL))
    {
        std::cerr << "Error occured while trying to send SetOutputConfiguration message." << std::endl;
        return 7;
    }

    unsigned char wBuff3[] = {0xFA,0xFF,0x10,0x00,0xF1};	// This is the GoToMeasurment message
    dwBytesWritten = 0;

    if (!WriteFile(hSerial, wBuff3, 6, &dwBytesWritten, NULL))
    {
        std::cerr << "Error occured while trying to send GoToMeasurment message." << std::endl;
        return 8;
    }
    
    configured = true;
    return 0;
}

bool AHRSInputIOProcessor::loopCondition()
{
    return true;
}

void AHRSInputIOProcessor::loop()
{
    if (!configured)
        configAHRS();

    unsigned char szBuff[64 + 1] = {0}; // Buffer size
    DWORD dwBytesRead = 0; // Number of bytes actually read

    if (!ReadFile(hSerial, szBuff, 64, &dwBytesRead, NULL))
    {
        std::cerr << "Error occured while trying to read bytes." << std::endl;
    }

    int index = -1;
    for (int i=0; i < 32; i++)	// This gets the bytes for the Yaw angle
    {
        if (szBuff[i] == 0xFA && szBuff[i+1] == 0xFF && szBuff[i+2] == 0x36)
        {
            index = i + 22;
            break;
        }
    }

    unsigned char rollArray[8];
    unsigned char pitchArray[8];
    unsigned char yawArray[8];
    if (index != -1) // This puts all those bytes 
    {
        for (int i = 0; i < 8; i++)
        {
            rollArray[i] = szBuff[index - (i + 8)];
            pitchArray[i] = szBuff[index - i];
            yawArray[i] = szBuff[index + (8 - i)];
        }
    }
    
    double doubleRoll = *reinterpret_cast<double* const>(rollArray);
    double doublePitch = *reinterpret_cast<double* const>(pitchArray);
    double doubleYaw = *reinterpret_cast<double * const>(yawArray);
    Attribute attRoll("rolAngle", 8, &doubleRoll);
    getComms()->sendAttribute(attRoll);
    Attribute attPitch("pitAngle", 8, &doublePitch);
    getComms()->sendAttribute(attPitch);
    Attribute attYaw("yawAngle", 8, &doubleYaw);
    getComms()->sendAttribute(attYaw);

    // I am not sure if it will close autimatically when the process ends, at the moment I guess we just won't close it...
    //CloseHandle(hSerial);
}