TruePoint.cpp

#include "TruePoint.h"

static void ComTask(TruePoint *truePoint) {
    while (true) {
        truePoint->GetBuffer();
        Wait(0.005);
    }
}

TruePoint::TruePoint():
    com(9600),
    comTask("Compass", (FUNCPTR)ComTask)
{
}

void TruePoint::Start() {
    comTask.Start();
}

void TruePoint::Stop() {
    comTask.Stop();
}

void TruePoint::GetBuffer() {
    if (com.GetBytesReceived() == 0) return;

    char *byte;
    com.Read(byte, 1);
    buffer.push_back(*byte);
    if (*byte == 0x7E)
        ProcessBuffer();
}

void TruePoint::ProcessBuffer() {
    
    int tail = buffer.size() - 1;
    if (buffer.at(tail - 1) != 0x0A || buffer.at(tail - 2) != 0x0D) return;

    for (size_t i = 0; i < buffer.size() - 3; i++)
        packet.push_back(buffer.at(i));

    buffer.clear();
    ProcessPacket();
}

void TruePoint::ProcessPacket() {
    if (packet.at(0) != 0x70) return;
    curByte = packet.begin() + 2;
    roll = GetKang();
    pitch = GetKang();
    azimuth = GetKang();
    accelX = GetInt();
    accelY = GetInt();
    accelZ = GetInt();
    magnetX = GetInt();
    magnetY = GetInt();
    magnetZ = GetInt();
}

float TruePoint::GetKang() {
    return GetInt() / 180.0;
}

int TruePoint::GetInt() {
    char a = *(curByte++);
    char b = *(curByte++);

    // Little endian, least significant (tail) byte first.  Shift the second
    // byte one byte to the left, then OR the bytes to concatenate.
    UINT16 raw = a | (b << 8);

    // Bitmask the most significant bit to get the sign.
    bool isNegative = raw & 32768;

    // Set the most significant bit to 0.
    raw &= 65535;

    int value = (int)raw;
    if (isNegative) value *= -1;

    return value;
}