File content as of revision 6:77a4c45f6416:

#include "Packet.h"

Packet::Packet()
{
    this->NUM = 0;
    this->IDD = 0;
    this->PWM = 0;
}

void Packet::addValues(uint8_t num,uint8_t destID,uint8_t pwm, uint8_t ownID,int16_t temp)
{
    this->NUM = num +1;
    this->IDD = destID;
    this->PWM = pwm;
    this->IDN.push_back(ownID);
    this->TMP.push_back(temp);
    //this->CRC = crc;
    printf("ADDED %5s, %5s, %5s, %5s, %5s\r\n","Numb","DesID","PWM","OwnID","Temp");
    printf("ADDED %5d, %5d, %5d, %5d, %5d\r\n",num,destID,pwm,ownID,temp); 
}

void Packet::dataInput(std::string stri)
{
    std::string str = stri;
    NUM = str[2];
    printf("NO of mbeds visited= %d\r\n", NUM);
    IDD = str[3];
    printf("Destination ID= %d\r\n", IDD);
    PWM = str[4];
    printf("PWM for the LED= %d\r\n", PWM);

    int frameNumber = 5;
    IDN.clear();
    printf("IDN=");
    for( int i = 0; i < NUM; i++ ) {
        IDN.push_back(str.at(frameNumber));
        printf(" %d=%d  ",i ,str.at(frameNumber));
        frameNumber++;
    }
    printf("\r\n");
    TMP.clear();
    printf("TMP=");
    for( int i = 0; i < NUM; i++ ) {
        //for( int j = 0; j < 2; j++ ) {
        printf("%c and %c) =",str.at(frameNumber+1),str.at(frameNumber));
        int16_t data = (str.at(frameNumber) << 8) | str.at(frameNumber+1);
        frameNumber = frameNumber +2;

        printf("%d= %d ",i ,data);
        TMP.push_back(data);
        //}
    }
    printf("\r\n");
    CRC = str[frameNumber];
}

std::string Packet::toDataString()
{
    std::string test;
    test.push_back(FRAME_BYTE_1);
    test.push_back(FRAME_BYTE_2); 
    string crcString = Packet::crcString();
    test.append(crcString);
    test.push_back(Packet::crc(crcString,0));
    test.push_back(FRAME_BYTE_2);
    test.push_back(FRAME_BYTE_1);
    printf("chars in stringmaker: ");
    for (int a = 0; a<test.size(); a++) {
            printf("%d\r\n",test.at(a));
        }
    return test;
}



std::string Packet::crcString()
{
    std::string test;
    test.push_back(NUM);
    test.push_back(IDD);
    test.push_back(PWM);
    //printf("Sending - mbeds visited= %d\r\n", NUM);
    //printf("Sending - destination ID= %d\r\n", IDD);
    //printf("Sending - PWM= %d\r\n", PWM);
    for( int i = 0; i < IDN.size(); i++ ) {
        test.push_back(IDN.at(i));
        //printf("Sending - IDN %d= %d\r\n",i ,test[5+i]);
    }

    for( int j = 0; j< TMP.size(); j++ ) {
        int16_t data = TMP.at(j);
        test.push_back((data >> 8) & 0xff);
        test.push_back(data & 0xff);
    }
    return test;
}


bool Packet::crcCheck()
{
    return 0 == Packet::crc(Packet::crcString(),CRC);
}

bool Packet::idCheck(int ID)
{
    for( int i = 0; i < IDN.size(); i++ ) {
        if(ID == IDN.at(i)){
            return false;
        }
    }
    return true;
}


/** 
 *  EXAMPLE CRC CHECK : 0xAA00FF55 => checksum = 0x39
 *  0xAA          0x00              0xFF            0x55
 *  1 0 1 0 1 0 1 0 0 0 0 0 0 0 0 0 1 1 1 1 1 1 1 1 0 1 0 1 0 1 0 1 | 0 0 0 0 0 0
 *  1 0 1 0 0 1 1 | | | | | | | | | | | | | | | | | | | | | | | | | | | | | | | |
 *  0 0 0 0 1 1 0 0 0 0 0 | | | | | | | | | | | | | | | | | | | | | | | | | | | |
 *          1 0 1 0 0 1 1 | | | | | | | | | | | | | | | | | | | | | | | | | | | |
 *          0 1 1 0 0 1 1 0 | | | | | | | | | | | | | | | | | | | | | | | | | | | //102
 *            1 0 1 0 0 1 1 | | | | | | | | | | | | | | | | | | | | | | | | | | | 
 *            0 1 1 0 1 0 1 0 | | | | | | | | | | | | | | | | | | | | | | | | | | //106
 *              1 0 1 0 0 1 1 | | | | | | | | | | | | | | | | | | | | | | | | | |
 *              0 1 1 1 0 0 1 0 | | | | | | | | | | | | | | | | | | | | | | | | | //114
 *                1 0 1 0 0 1 1 | | | | | | | | | | | | | | | | | | | | | | | | |
 *                0 1 0 0 0 0 1 0 | | | | | | | | | | | | | | | | | | | | | | | | //66
 *                  1 0 1 0 0 1 1 | | | | | | | | | | | | | | | | | | | | | | | |
 *                  0 0 1 0 0 0 1 0 1 | | | | | | | | | | | | | | | | | | | | | | //69
 *                      1 0 1 0 0 1 1 | | | | | | | | | | | | | | | | | | | | | |
 *                      0 0 1 0 1 1 0 1 1 | | | | | | | | | | | | | | | | | | | | //91
 *                          1 0 1 0 0 1 1 | | | | | | | | | | | | | | | | | | | |
 *                          0 0 0 1 0 0 0 1 1 1 | | | | | | | | | | | | | | | | | //71
 *                                1 0 1 0 0 1 1 | | | | | | | | | | | | | | | | |
 *                                0 0 1 0 1 0 0 1 1 | | | | | | | | | | | | | | | // 83
 *                                    1 0 1 0 0 1 1 | | | | | | | | | | | | | | |
 *                                    0 0 0 0 0 0 0 0 1 0 1 0 1 0 1 | | | | | | | //85
 *                                                    1 0 1 0 0 1 1 | | | | | | |
 *                                                    0 0 0 0 1 1 0 | 0 0 0 0 | | //96
 *                                                            1 0 1 | 0 0 1 1 | |
 *                                                            0 1 1 | 0 0 1 1 0 | //102
 *                                                              1 0 | 1 0 0 1 1 |
 *                                                              0 1 | 1 0 1 0 1 0 //106
 *                                                                1 | 0 1 0 0 1 1
 *                                                                0 | 1 1 1 0 0 1 //57 = 0x39
 * using this algorithm gives the same values.
 */
unsigned char Packet::crc(std::string message, char chk)
{
    int nBytes = message.size();
    char remainder = 0;
    /**
     * Shifts the checksum 2 bits to the right,
     * the checksum is of the form 0b00xx xxxx,
     * so this has to get shifted to 0bxxxx xx00 to do the check
     */
    char check = chk<<2;
    
    /*
    * Perform modulo-2 division, bit by bit, byte by byte
    */

    for (int byte = 0; byte < nBytes; byte++)
    {
        for (char bit = 8; bit > 0; --bit)
        {
            /*
            * Try to divide the current data bit.
            */
            if (remainder & 0x40)
            {
                remainder = (remainder ^ 0x53) & 0x7F;
                remainder = (remainder << 1) | (0x01 & (message.at(byte)>>(bit-1)) & 0x7F);
                printf("Other remainder %d %d\r\n", remainder, byte);
            }
            else
            {
                remainder = ((remainder << 1) | (0x01 & (message.at(byte)>>(bit-1))) & 0x7F);
                printf("Shifted remainder dqfs %d\r\n", message[byte]);
                printf("Shifted remainder %d %d\r\n", remainder, byte);
            }
        }
    }

   /*
    * Perform modulo-2 division, this for only the 6 bits of the chk value
    * will be performed 7 times but the trailing 0 will be removed later
    */
    for (char bit = 7; bit > 0; --bit)
        {
            /*
            * Try to divide the current data bit.
            */
            if (remainder & 0x40)
            {
                remainder = (remainder ^ 0x53) & 0x7F;
                remainder = (remainder << 1) | (0x01 & (check >> bit)) & 0x7F;
                printf("%d %d\r\n",bit,(0x01 & (check >> bit)));
                printf("Other remainder %d\r\n", remainder);
            }
            else
            {
                remainder = ((remainder << 1) | (0x01 & (check >> bit))) & 0x7F;
                printf("%d %d\r\n",bit,(0x01 & (check >> bit)));
                printf("Shifted remainder %d\r\n", remainder);
            }
        }
        /*
         *  Remove the trailing 0 that has been shifted and has to be removed
         */
        remainder = remainder >>1;
    return (remainder);

}


void Packet::setNUM(uint8_t NUM)
{
    this->NUM = NUM;
}

uint8_t Packet::getNUM()
{
    return this->NUM;
}

void Packet::setIDD(uint8_t IDD)
{
    this->IDD = IDD;
}

uint8_t Packet::getIDD()
{
    return this->IDD;
}

void Packet::setCRC(uint8_t CRC)
{
    this->CRC = CRC;
}

uint8_t Packet::getCRC()
{
    return this->CRC;
}
void Packet::setPWM(uint8_t PWM)
{
    this->PWM = PWM;
}
uint8_t Packet::getPWM()
{
    return this->PWM;
}
void Packet::setIDN(vector<uint8_t> IDN)
{
    this->IDN = IDN;
}
vector<uint8_t> Packet::getIDN()
{
    return this->IDN;
}
void Packet::setTMP(vector<int16_t> TMP)
{
    this->TMP = TMP;
}
vector<int16_t> Packet::getTMP()
{
    return this->TMP;
}

/*
unsigned char Packet::crcCalculate()
{
    std::string test;
    test.append(Packet::crcString());
    return Packet::calculateCrc(test);
}

unsigned char Packet::calculateCrc(std::string str)
{
    int i = 0;
    char poly = 0x53; //1010011 
    int bits = str.size()*8;
    char *msg= new char[str.length() + 1];
    strcpy(msg, str.c_str());
    //unsigned char crc = 0xfc;
    unsigned char crc = 0x00;

    while(bits--) {
        if(!i--) {
            i = 7;
            crc ^= *msg++;
        }
        crc = crc << 1 ^ (crc & 0x80 ? poly : 0);
    }
    return(crc >> 2 & 0x3f);

}
*/