TruePoint.cpp

#include "TruePoint.h"

#define INT(name) (name) = GetInt(); printf("(name) int")
#define ANGLE(name) (name) = GetKang(); printf("(name) angle")

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

TruePoint::TruePoint():
    com(9600),
    comTask("Compass", (FUNCPTR)ComTask),
    startSequence()
{
    startSequence[0] = 0x0D;
    startSequence[1] = 0x0E;
    startSequence[2] = 0x7E;
    sequenceSum = startSequence[0] + startSequence[1] + startSequence[2];
}

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

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

void TruePoint::Process() {
    printf("Checking number of bytes received.\n");
    if (com.GetBytesReceived() == 0) return;

    printf("Reading a new byte.\n");
    char curByte;
    com.Read(&curByte, 1);

    printf("Check the byte against our start sequence.\n");
    // Check if we're continuing our header sequence.
    if (curByte == startSequence[expectedSequenceByte]) {
        expectedSequenceByte++;
        printf("Incremented our expected sequence byte.\n");
        if (expectedSequenceByte == SEQUENCE_LENGTH) {
            printf("Finished our start sequence, new packet.\n");
            // Ready for a new packet.
            expectedSequenceByte = 0;
            packetByteCount = 0;
            dataSize = 0;
            packetSum = sequenceSum;
            packet.clear();
            return;
        }
    }

    printf("Checking which byte we have.\n");
    printf("Byte number: %d - Sum: %d - Data Size: %d\n", packetByteCount, packetSum, dataSize);
    if (packetByteCount == 0)                 // Our packet/command ID.
        currentPacketID = curByte;
    else if (packetByteCount == 1)            // The number of data bytes.
        dataSize = (int)curByte;
    else if (packetByteCount == dataSize + 2) // Trailer byte?
        if ((int)curByte == packetSum % 256)  // Checksum.
            ProcessPacket();
    else
        packet.push_back(curByte);

    printf("Summing byte and incrementing byte count.\n");
    packetSum += (int)curByte;
    packetByteCount++;
}

void TruePoint::ProcessPacket() {
    printf("Processing the packet.\n");
    packetIter = packet.begin();
    {
        ANGLE(roll);
        ANGLE(pitch);
        ANGLE(azimuth);
        INT(accelY);  // Rightward acceleration
        INT(accelX);  // Forward acceleration
        INT(accelZ);  // Upward acceleration
        INT(magnetY); // Rightward magnetometer
        INT(magnetX); // Forward magnetometer
        INT(magnetZ); // Upward magnetometer
    }
}

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

int TruePoint::GetInt() {
    char a = *(packetIter++);
    char b = *(packetIter++);
    printf("Bytes: 0x%X 0x%X\n", a, b);

    // 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);
    printf("Value: 0x%X\n", raw);

    // Bitmask the most significant bit to get the sign.
    bool isNegative = raw & 32768;
    printf("Is negative? %d", isNegative);

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

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

TruePoint.h

#ifndef _TRUE_POINT_H_
#define _TRUE_POINT_H_

#include "WPILib.h"
#include "SerialPort.h"
#include <vector>

#define SEQUENCE_LENGTH 3

typedef vector<char> Packet;
typedef vector<char>::iterator PacketIter;

class TruePoint {
private:
    SerialPort com;
    Task comTask;

    char startSequence[SEQUENCE_LENGTH];
    int expectedSequenceByte;
    int sequenceSum;

    Packet packet;
    PacketIter packetIter;
    int packetByteCount;
    int dataSize;
    int currentPacketID;
    int packetSum;

    float GetKang();
    int GetInt();
    void ProcessPacket();


public:
    TruePoint();
    void Process();
    void Start();
    void Stop();

    float roll,
          pitch,
          azimuth;

    int accelX,
        accelY,
        accelZ,
        magnetX,
        magnetY,
        magnetZ;
};

#endif