File content as of revision 4:466d859bfb13:

#include "mbed.h"
#include "Frame.h"
#include "lib_crc.h"
//SOF LEN IDD TMP PWM ID0 ID1 … IDn CRC EOF
//SOF (16bit): start of frame (0xAA 0xBB)
//LEN (8bit): lengte van frame in bytes (volledige frame!)
//IDD (8bit): ID destinationmbed(1-15)
//TMP (16bit): 0b0000XXXX 0bXXXXXXXX Temperature(0 –4095) (-25°tot 85°)
//PWM (8bit): PWM dutycycle(0 –255) (0% –100%)
//TUN (8bit): Select tune & delay (1 –15) zie tabel 1
//ID0–IDn(n x 8bit): lijst van reeds bezochte mbeds
//CRC (16bit): CRC16 checksum(x16+ x15+ x2+ 1) checksumop “LEN IDD TMP PWM ID0…IDn”
//EOF (16bit): end of frame (0xCC 0xDD)
Frame::Frame()
{
    IdDestinationMbed=0;
    temperature=0;
    pWMDutycycle=0;
    tune=0;
    lengthIDs=0;
    IDs= new char[0];
}
Frame::Frame(    int IdDestinationMbed,
                 int temperature,
                 int pWMDutycycle,
                 int tune,
                 int lengthIDs,
                 char* IDs)
{
    this->IdDestinationMbed=IdDestinationMbed;
    this->temperature=temperature;
    this->pWMDutycycle=pWMDutycycle;
    this->tune=tune;
    this->lengthIDs=lengthIDs;
    this->IDs=IDs;
}

int Frame::Decode(char* frame)
{
    int length = (int)frame[2];
    lengthIDs = length- 12;
    IDs = new char[lengthIDs];

    if ( frame[0]==0xAA && frame[1]==0xBB && frame[length-2]==0xCC && frame[length-1]==0xDD) {

        IdDestinationMbed = (int) frame[3];
        temperature = (int) ((frame[4]<<8) | frame[5])  ;
        pWMDutycycle = (int)frame[6];
        tune = (int) frame [7];
        crc= (int) ((frame[length-4]<<8) | frame[length-3]);
        for (int i=0; i<lengthIDs; i++) {
            IDs[i]=frame[i+8];
        }

        printf("Decode length: %d IdDestinationMbed: %d temperature: %d \n\r",length,IdDestinationMbed,temperature);
        printf("pWMDutycycle: %d tune: %d \n\r",pWMDutycycle,tune);
        for(int i=0; i<lengthIDs; i++) {
            printf("IDs %d \n\r",IDs[i]);
        }
    }
    return CheckCRC();
}

int Frame::Encode(char* encode)
{
    memset(encode, 0, 255);
    int length = 12+lengthIDs;

    encode[0]=0xAA;
    encode[1]=0xBB;
    encode[2]= (char) length;
    encode[3]= (char) IdDestinationMbed;
    encode[4]= (char) (temperature>>8);
    encode[5]= (char) temperature;
    encode[6]= (char) pWMDutycycle;
    encode[7]= (char) tune;

    for (int i=0; i<lengthIDs; i++) {
        encode[8+i]=IDs[i];
    }

    // Calculate CRC
    int CRCint = MakeCRC(encode[2],encode[3],encode[4],encode[5],encode[6],IDs,lengthIDs);

    encode[length-4]= ( char)(CRCint>>8);
    encode[length-3]=( char)(CRCint);

    encode[length-2]=0xCC;
    encode[length-1]=0xDD;
    return length;
}

int Frame::MakeCRC(
    char LEN, //lenght of frame
    char IDD, //Destination ID
    char TMP0,//first tmp char
    char TMP1,//second tmp char
    char PWM, //pwm tune
    char* data, //data frame
    int lenghtData //lenght of the data
)
{
    //char* crcData = new char[5+lenghtData] ;
    char crcData[5+lenghtData] ;

    crcData[0] = LEN; //lenght of frame
    crcData[1] = IDD; //Destination ID
    crcData[2] = TMP0;//first tmp char
    crcData[3] = TMP1;//second tmp char
    crcData[4] = PWM; //pwm tune

    // add the data to the crcData
    for (int i=0; i<lenghtData; i++) {
        crcData[i+5]= data[i];
    }
    return calculate_crc16(crcData, lenghtData+5);
}

void Frame::testEncode(char* frame)
{
    printf("testEncode\n\r");
    char controle[]= {0xAA,0xBB,0x0F,0x0B,0x0F,0xA0,0x02,0x05,0x08,0x09,0x0A,0x4B,0x7C,0xCC,0xDD};
    //                 SOF  SOF  LEN  IDD  TMP  TMP  PWM  TUN  ID0  ID1  ID2  CRC CRC  EOF  EOF
    for (int i=0; i<15; i++) {
        // loop all chars and check if there right
        if(controle[i]!=frame[i]) {
            printf("ERROR in frame %d : %X != %X\n\r",i,(unsigned char) controle[i],(unsigned char)frame[i] );
        }
    }
    printf("End testEncode\n\r");
}
int Frame::CheckCRC()
{
    if(crc == MakeCRC((char)(12+lengthIDs),(char)IdDestinationMbed,(char)(temperature>>8),(char)temperature,(char)pWMDutycycle,(char* )IDs,lengthIDs))
        return 1;
    else return 0;
}
void Frame::AddID(int id)
{
    lengthIDs++;
    char* IDs2 = new char[lengthIDs];
    for(int i=0; i<lengthIDs; i++) {
        IDs2[i] = IDs[i];
    }
    IDs2[lengthIDs-1]=id;

    char* tempIDs=IDs;
    IDs=IDs2;
    delete tempIDs;
}

int Frame::CheckMyID(int id)
{
    for(int i=0; i<lengthIDs; i++) {
        if(id == IDs[i])
            return 1;
    }
    return 0;
}

void Frame::setIdDestinationMbed(int IdDestinationMbed)
{
    this->IdDestinationMbed=IdDestinationMbed;
}
int Frame::getIdDestinationMbed(void)
{
    return IdDestinationMbed;
}
void Frame::setTemperature(float temperature)
{
    this->temperature=(int) (37.227*(temperature+25));
}
int Frame::getTemperature(void)
{
    return (int) (0.026862*temperature-25);
}
void Frame::setPWMDutycycle(float pWMDutycycle)
{
    this->pWMDutycycle=(int) (pWMDutycycle*255);
}
void Frame::setTune(int tune)
{
    this->tune=tune;
}
int Frame::getTune(void)
{
   return tune;
}
void Frame::setLengthIDs(int lengthIDs)
{
    this->lengthIDs=lengthIDs;
}
void Frame::setIDs(char* IDs)
{
    this->IDs=IDs;
}