File content as of revision 2:7f7eb0cc78a4:

#include "Packet.h"
#include "mbed.h"

Serial pp(USBTX, USBRX);

//Constructor & Destructor
Packet::Packet(uint8_t ID[2],uint8_t* packet,int length)
{
    //geheugen in de heap vrij maken
    this->ID = new uint8_t[2];
    this->RESERVED = new uint8_t[2];
    this->nBytesFilled = 0;

    if(length < 9 || packet[2]!= ID[0] || packet[3] != ID[1])
    {
        this->TOT_SIZE = 7;
        this->ID[0] = ID[0];
        this->ID[1] = ID[1];
        this->CMD = PACKET_ERROR;
        this->CMD_SIZE = 0;
        this->CMD_DATA = NULL;
        this->RESERVED[0] = 0;
        this->RESERVED[1] = 0;
    }
    else
    {
        this->TOT_SIZE = (packet[0] << 8) + packet[1];
        this->ID[0] = packet[2];
        this->ID[1] = packet[3];
        this->CMD = packet[4];
        this->CMD_SIZE = (packet[5] << 8) + packet[6];
        this->CMD_DATA = new uint8_t[this->CMD_SIZE];
        for (int i = 7; i < 7+this->CMD_SIZE; i++)
        {
            this->CMD_DATA[i-7] = packet[i];
        }
        this->RESERVED[0] = packet[7+this->CMD_SIZE];
        this->RESERVED[1] = packet[7+this->CMD_SIZE+1];
    }
}
Packet::Packet(uint8_t ID[2])
{
    //geheugen in de heap vrij maken
    this->ID = new uint8_t[2];
    this->RESERVED = new uint8_t[2];
    this->nBytesFilled = 0;
    
    this->TOT_SIZE = 7;
    this->ID[0] = ID[0];
    this->ID[1] = ID[1];
    this->CMD = PACKET_ERROR;
    this->CMD_SIZE = 0;
    this->CMD_DATA = NULL;
    this->RESERVED[0] = 0;
    this->RESERVED[1] = 1;
}
Packet::Packet(uint8_t ID[2],uint8_t CMD,uint8_t RESERVED[2])
{
    //geheugen in de heap vrij maken
    this->ID = new uint8_t[2];
    this->RESERVED = new uint8_t[2];
    this->nBytesFilled = 0;
    
    this->TOT_SIZE = 7;
    this->ID[0] = ID[0];
    this->ID[1] = ID[1];
    this->CMD = CMD;
    this->CMD_SIZE = 0;
    this->CMD_DATA = NULL;
    this->RESERVED[0] = RESERVED[0];
    this->RESERVED[1] = RESERVED[1];
}
Packet::Packet(uint8_t ID[2],uint8_t CMD,uint8_t* data,int datasize,uint8_t RESERVED[2])
{
    //geheugen in de heap vrij maken
    this->ID = new uint8_t[2];
    this->RESERVED = new uint8_t[2];
    this->nBytesFilled = 0;
    
    this->TOT_SIZE = datasize + 7;
    this->ID[0] = ID[0];
    this->ID[1] = ID[1];
    this->CMD = CMD;

    this->CMD_SIZE = datasize;
    this->CMD_DATA = data;
    this->RESERVED[0] = RESERVED[0];
    this->RESERVED[1] = RESERVED[1];
}
//this is a special constructor that makes a data packet,
//wich can be filled later with measurement data
Packet::Packet(uint8_t ID[2],unsigned int firstSampleNumber,uint8_t nSamples,uint8_t sampleRate ,int activeChannels,int nActiveChannels,uint8_t RESERVED[2])
{
    //geheugen in de heap vrij maken
    this->ID = new uint8_t[2];
    this->RESERVED = new uint8_t[2];
    this->nBytesFilled = 8;
    
    this->TOT_SIZE = 15 + nSamples * nActiveChannels * 2;
    this->ID[0] = ID[0];
    this->ID[1] = ID[1];
    this->CMD = PACKET_DATA;

    this->CMD_SIZE = 8 + nSamples*nActiveChannels * 2;
    this->CMD_DATA = new uint8_t[this->CMD_SIZE];
    this->CMD_DATA[0] = (firstSampleNumber >> 24) & 0xff;
    this->CMD_DATA[1] = (firstSampleNumber >> 16) & 0xff;
    this->CMD_DATA[2] = (firstSampleNumber >> 8) & 0xff;
    this->CMD_DATA[3] = (firstSampleNumber) & 0xff;
    this->CMD_DATA[4] = nSamples;
    this->CMD_DATA[5] = sampleRate;
    this->CMD_DATA[6] = (activeChannels >> 8) & 0xff;
    this->CMD_DATA[7] = (activeChannels) & 0xff;
    for (int i = 8; i < this->CMD_SIZE; i++)
    {
        this->CMD_DATA[i] = 0;
    }
    this->RESERVED[0] = RESERVED[0];
    this->RESERVED[1] = RESERVED[1];
}
Packet::~Packet(void)
{
    delete [] ID;
    delete [] CMD_DATA;
    delete [] RESERVED;
}

//GETTERS
int Packet::getTOT_SIZE(void)
{
    return(this->TOT_SIZE);
}
uint8_t* Packet::getID(void)
{
    return(this->ID);
}
uint8_t  Packet::getCMD(void)
{
    return(this->CMD);
}
int Packet::getCMD_SIZE(void)
{
    return(this->CMD_SIZE);
}
uint8_t* Packet::getCMD_DATA(void)
{
    return(this->CMD_DATA);
}
uint8_t* Packet::getRESERVED(void)
{
    return(this->RESERVED);
}
int Packet::getBytesFilled(void)
{
    return(this->nBytesFilled);
}

//SETTERS
void Packet::setID(uint8_t ID[2])
{ 
    this->ID[0] = ID[0];
    this->ID[1] = ID[1];
}
void Packet::setCMD(uint8_t CMD)
{
    this->CMD = CMD;
}
void Packet::setDATA(uint8_t* packetdata,int length)
{
    delete [] CMD_DATA;
    this->CMD_DATA = new uint8_t[length];
    for (int i = 0; i < length; i++)
    {
        this->CMD_DATA[i] = packetdata[i];
    }

    this->CMD_SIZE = length;
    this->TOT_SIZE = length+7;
}
void Packet::setRESERVED(uint8_t RESERVED[2])
{
    this->RESERVED[0] = RESERVED[0];
    this->RESERVED[1] = RESERVED[1];
}
bool Packet::addMeasurement(Measurement *m)
{
    if(nBytesFilled!=0)
    {
        if(nBytesFilled<this->CMD_SIZE)
        {
            //de eerste 8 kanalen bekijken..
            for(int i=7;i>=0;i--)
            {
                if(((this->CMD_DATA[6] >> i) & 0x01) == 1)
                {
                    this->CMD_DATA[nBytesFilled] = (m->getPunt(7-i)>>8) & 255;
                    this->nBytesFilled++;
                    this->CMD_DATA[nBytesFilled] = m->getPunt(7-i) & 255;
                    this->nBytesFilled++;
                }            
            }
            for(int i=7;i>=0;i--)
            {
                if(((this->CMD_DATA[7] >> i) & 0x01) == 1)
                {
                    this->CMD_DATA[nBytesFilled] = (m->getPunt(15-i)>>8) & 255;
                    this->nBytesFilled++;
                    this->CMD_DATA[nBytesFilled] = m->getPunt(15-i) & 255;
                    this->nBytesFilled++;
                }            
            }
            //Wordt nu gedelete in meting thread!
            //delete m;
            return(true);
        }
    }
    //Wordt nu gedelete in meting thread!
    //delete m;
    return(false);
}