Comm.cpp

//------------------------------------------------------------------------------
//This file is part of embKernel. 
//See license.txt for the full license governing this code.
//------------------------------------------------------------------------------

#include "Comm.hpp"
#include "Hw.hpp"

Comm::Comm(int priority, size_t stackSize, uint8_t outEp, uint8_t inEp) :
        mTask(priority, "Comm", stackSize, *this), mUsbRxStream(outEp, 128), mUsbTxStream(inEp, 128) {

}

Comm::~Comm() {

}

void Comm::run() {
    Hw::sLsm303dlhc.init();

    while (true) {
        while (true) {
            LibPktHandler::PKT* pkt;
            LibPktHandler::RESULT result = mPktHandler.recvPkt(mUsbRxStream, &pkt, Rtos::convertMsToTick(1000));
            if (result == LibPktHandler::RES_SUCCESS) {
                switch (pkt->hdr.cmd) {
                    case CMD_GET_LEDS: {
                        uint8_t buffer[8];
                        buffer[0] = Hw::sLed3;
                        buffer[1] = Hw::sLed4;
                        buffer[2] = Hw::sLed6;
                        buffer[3] = Hw::sLed8;
                        buffer[4] = Hw::sLed10;
                        buffer[5] = Hw::sLed9;
                        buffer[6] = Hw::sLed7;
                        buffer[7] = Hw::sLed5;
                        mPktHandler.sendPkt(mUsbTxStream, CMD_GET_LEDS, sizeof(buffer), buffer);
                        break;
                    }
                    case CMD_SET_LEDS: {
                        Hw::sLed3 = pkt->data[0];
                        Hw::sLed4 = pkt->data[1];
                        Hw::sLed6 = pkt->data[2];
                        Hw::sLed8 = pkt->data[3];
                        Hw::sLed10 = pkt->data[4];
                        Hw::sLed9 = pkt->data[5];
                        Hw::sLed7 = pkt->data[6];
                        Hw::sLed5 = pkt->data[7];
                        mPktHandler.sendPkt(mUsbTxStream, CMD_SET_LEDS, 4, pkt->data);
                        break;
                    }
                    case CMD_GET_ACC_XYZ: {
                        float xyz[3];
                        Hw::sLsm303dlhc.readAccXyz(&xyz[0], &xyz[1], &xyz[2]);
                        mPktHandler.sendPkt(mUsbTxStream, CMD_GET_ACC_XYZ, sizeof(xyz), &xyz);
                    }
                    default:
                        break;
                }
                mPktHandler.freePkt(&pkt);
            }
            else {
                mPktHandler.reset();
            }
        }
    }
}