source: tmcsimulator/branches/WorkingFEPSimulator/DataPacker.cpp @ 107

#include "DataPacker.h";



unsigned char * DataPacker::msg;



unsigned char * DataPacker::packData(STATION *station) {

    int pos = 26;

    DataPacker packer;

    msg = packer.staticDataPack(station);

    pos = packer.dynamicDataPack(station, 5, pos);    

    pos = packer.dynamicDataPack(station, 6, pos);

    pos = packer.dynamicDataPack(station, 7, pos);

    pos = packer.dynamicDataPack(station, 8, pos);

    // Update data for BYTE 9, 16, 21, 22, 23, 24



    // what is the current metering rate?

    // check each LDS, if there is demand detector, the downstream node the

    // ldsMap[i].dataPack[22-1] = ldsMap[i].dataPack[9-1] = ;



    // will need to check if queue override is enabled (Currently, no queue

    // ldsMap[i].dataPack[16-1] = ;



    // byte 23: MAINLINE total volume

    msg[23 - 1] = station->MlTotVol;



    // byte 24: opposite total volume

    msg[24 - 1] = station->OppTotVol;



    // last BYTE: checksum 

    msg[station->length - 1] =

            packer.chksum(msg, station->length - 1 - 1);

    

    /*

    for (int j = 0; j < station->length; j++) {

        printf("%02X", msg[j]);

    }

    printf("\n");

    */

}



int DataPacker::dynamicDataPack(STATION *station, int packNo, int pos) {

    int i, j, vol, occ, haveData;

    //used for data byte 23 and 24

    station->MlTotVol = 0;

    station->OppTotVol = 0;



    for (j = 1; j <= 8; j++) {

        

        if (DataAvail(msg[packNo - 1], j)) {

            // loop at bit j has data

            for (int lane = 0; lane < station->loops.size(); lane++) {

                // what's the corresponding loop of bit j

                char * currLoopLoc = station->loops.at(lane)->loop_loc;

                if (packNo == 5)

                    haveData = strcmp(currLoopLoc, dp5[j - 1]);

                else if (packNo == 6)

                    haveData = strcmp(currLoopLoc, dp6[j - 1]);

                else if (packNo == 7)

                    haveData = strcmp(currLoopLoc, dp7[j - 1]);

                else if (packNo == 8)

                    haveData = strcmp(currLoopLoc, dp8[j - 1]);

                else

                    return -1;



                if (haveData == 0) {

                    LOOP *currLoop = station->loops.at(lane);

                    vol = currLoop->vol;

                    occ = (int) (currLoop->occ * 900 + 0.5);



                    VOLOCC packedVOLOCC = packVOLOCC(vol, occ);

                    pos++;

                    msg[pos - 1] = packedVOLOCC.high;

                    pos++;

                    msg[pos - 1] = packedVOLOCC.low;



                    if (packNo == 5) {

                        station->MlTotVol += vol;

                    } else if (packNo == 6) {

                        station->OppTotVol += vol;

                    }

                }

            }

        }

    }

    return pos;

}



unsigned char * DataPacker::staticDataPack(STATION *station) {

    int j;

    // Allocate memory for dataPack

    unsigned char *dataPack = (unsigned char *) calloc(sizeof(unsigned char), station->length);



    // dataPack 5-8: lane config

    char d5 = 0, d6 = 0, d7 = 0, d8 = 0;

    for (j = 0; j < station->loops.size(); j++) {

        for (int k = 0; k < 8; k++) {

            char * currLoopLoc = station->loops.at(j)->loop_loc;

            if (strcmp(currLoopLoc, dp5[k]) == 0)

                d5 += pow(2, k);

            if (strcmp(currLoopLoc, dp6[k]) == 0)

                d6 += pow(2, k);

            if (strcmp(currLoopLoc, dp7[k]) == 0)

                d7 += pow(2, k);

            if (strcmp(currLoopLoc, dp8[k]) == 0)

                d8 += pow(2, k);

        }

    }

    dataPack[5 - 1] = d5;

    dataPack[6 - 1] = d6;

    dataPack[7 - 1] = d7;

    dataPack[8 - 1] = d8;



    // dataPack 1: Drop number, i.e. station address

    dataPack[1 - 1] = station->drop;

    // dataPack2 (2 bytes per loop)

    dataPack[2 - 1] = station->loops.size() * 2 + Fixed_Byte_To_Checksum;



    // dataPacket 3 (lowbyte: # of mainline loops, highbyte: # of opposite loops)

    int low = 0, high = 0;

    for (j = 1; j <= 6; j++) {

        low += DataAvail(dataPack[5 - 1], j);

        high += DataAvail(dataPack[6 - 1], j);

    }

    high = high << 4;

    dataPack[3 - 1] = high | low;



    // dataPack4 (Miscl. flags: samples are: 80, A0, E0, 00)

    dataPack[4 - 1] = 0xA0;



    // dataPack 9: initialized as 00 (meaning no metering); need to be updated every 30 sec

    dataPack[9 - 1] = 0;



    // datadataPack 10-13: lane malfunction? Assuming all functional

    dataPack[10 - 1] = 0;

    dataPack[11 - 1] = 0;

    dataPack[12 - 1] = 0;

    dataPack[13 - 1] = 0;



    // dataPack 14-22: ramp metering data

    // BYTE 16 and 22 need to be updated every 30 sec

    bool found = false;

    for (j = 0; j < station->loops.size(); j++) {

        if (strcmp(station->loops.at(j)->loop_loc, "DEMAND") == 0) {

            found = true;

            break;

        }

    }

    if (found) {

        // BYTE 14: mostly 07, some are 05, 03, 00

        dataPack[14 - 1] = 0x07;

        // mostly 06(TOD table 1); some are 0B (No metering) or 05(traffic responsive)

        dataPack[15 - 1] = 0x06;

        // most 00, some are 01 (queue override) or 80(Meter ON sign)

        dataPack[16 - 1] = 0x00;

        // Field Manual Rate

        dataPack[17 - 1] = 0xFF;

        // TOC Manual Rate

        dataPack[18 - 1] = 0xFF;

        // PSO Manual Rate

        dataPack[19 - 1] = 0xFF;

        // CORM Rate

        dataPack[20 - 1] = 0xFF;

        // Local Responsive Rate. DON'T UNDERSTAND YET

        dataPack[21 - 1] = 0x00;

        // TOD Rate: need to query RAMP plugin!

        dataPack[22 - 1] = 0x00;

    }// LDS: NO Metering

    else {

        dataPack[14 - 1] = 0x00;

        dataPack[15 - 1] = 0x0B;

        dataPack[16 - 1] = 0x00;

        dataPack[17 - 1] = 0xFF;

        dataPack[18 - 1] = 0xFF;

        dataPack[19 - 1] = 0xFF;

        dataPack[20 - 1] = 0xFF;

        dataPack[21 - 1] = 0x00;

        dataPack[22 - 1] = 0x00;

    }



    // dataPack 23-24: sum of mainline/Opp traffic data; need to be updated every 30 sec

    station->MlTotVol = 0;

    station->OppTotVol = 0;

    dataPack[23 - 1] = station->MlTotVol;

    dataPack[24 - 1] = station->OppTotVol;



    // dataPack 25-26: BYTE 25 is fixed, i.e. 03; BYTE 26 is either 0xA2 or 0x84

    dataPack[25 - 1] = 0x03;

    dataPack[26 - 1] = 0x84;



    return dataPack;

}



bool DataPacker::DataAvail(char flag, int num) {

    int mag, fel;



    // find mask value

    if (num == 1)

        mag = 0x01;

    else if (num == 2)

        mag = 0x02;

    else if (num == 3)

        mag = 0x04;

    else if (num == 4)

        mag = 0x08;

    else if (num == 5)

        mag = 0x10;

    else if (num == 6)

        mag = 0x20;

    else if (num == 7)

        mag = 0x40;

    else if (num == 8)

        mag = 0x80;



    fel = flag & mag;

    fel = fel >> (num - 1);



    if (fel == 1)

        return true;

    else

        return false;

}



// checksum based on data from byte 1 (after 0DOA) to the second last byte (the 

char DataPacker::chksum(unsigned char *dataptr, int len) {

    int i;

    unsigned char checksum = 0;



    for (i = 0; i < len; i++)

        checksum += dataptr[i];



    return checksum;

}



// convert vol and occ data to a two-byte data packet

VOLOCC DataPacker::packVOLOCC(int vol, int occ)

{

   int bit, i;

   int   high, low; 

   int high_high = 0, high_low = 0;

   VOLOCC com;

   

   // occ data: 10 bits

   low = 0;

   for (i=9; i>=0; i--) 

   {

      if (i >= 8)

      {

         bit = ((occ >> i) & 1);

         high_low += pow(2, (i - 8)) * bit; 

      }

      // lower byte

      else

      {

         bit = ((occ >> i) & 1);

         low += pow(2, i) * bit;

      }

   }



   // vol: 6 bits (1. vol, shift to the left for two times; 2. combine data)

   high_high = vol << 2;

   high = high_high | high_low;



   com.high = high;

   com.low = low;



   return com;

}