fmviewer3/project/fm_viewer/data/rm_format_mov_emtudat.cpp

#include "rm_format_mov.h"
#include "fm_parse_gps.h"

#if (RM_MODEL == RM_MODEL_TYPE_XLDR_88)
#define GPSR_FLAG ('G' | 'P' << 8 | 'S' << 16 | 'R' << 24)
#define SENS_FLAG ('S' | 'E' << 8 | 'N' << 16 | 'S' << 24)
#define BYTE_SWAP_32(__num) ( ((__num>>24)&0xff) | ((__num<<8)&0xff0000) | ((__num>>8)&0xff00) | ((__num<<24)&0xff000000))
//#define GPRS_SENTENCE_SIZE      128

bool MOVFormat::parse_buffer(uint8_t* strd,long strdSize)
{
    size_t check_offset = 0;

    // GPSR 및 데이터 시작 위치가 다름 (이벤트에 따라)
    for(long i=0;i<strdSize;i++)
    {
        if(*((int32_t*)&strd[i]) == GPSR_FLAG || *((int32_t*)&strd[i]) == SENS_FLAG)
        {
            check_offset = i;
            break;
        }
    }
    if (check_offset == 0)
    {
        return false;
    }

    long offset = 0;
    int32_t flag;// = *((int32_t*)&strd[offset]);
    offset += (check_offset - 4); // GRSP 앞쪽 4BYTE 에 크기가 시작됨

    bool gpsDone = false;
    bool sensorDone = false;

    while(offset < strdSize)
    {
        // 데이터 크기는 GPS 및 센서 데이터 앞쪽에 있음
        int32_t dataSize = *((int32_t*)&strd[offset]);      // data to offset 이 가장 먼저 있음
        offset += 4;
        dataSize = BYTE_SWAP_32(dataSize);                  // eg. 4088 byte 부터 sensor data 시작, 센서는 9032

        int32_t nextOffset = dataSize + (check_offset - 4);  // 4088 + 24(front) = 4120 (file offset: )

        flag = *((int32_t*)&strd[offset]);                  // flag (type)
        offset += 4;

        int index = 0;

        if(flag == GPSR_FLAG)   // GPS Data
        {
            //_gps0Count = dataSize / (GPRS_SENTENCE_SIZE + 4 + 4);    // 136 = 0x88
            //qInfo() << "GPSR_FLAG found:" << _gpsCount;

            if(_nmea != NULL)
            {
                free(_nmea);
            }
            //_bGPSExist = true;
            NMEA_INFO* nmea = (NMEA_INFO*)malloc(sizeof(_NMEA_INFO) * 1000);
            _nmea = nmea;

//#if (DEBUG_SENSOR_DATA)
//            printf("gps data size:%d count:%d test:%f\n",dataSize,_gpsCount,(float)dataSize / (float)(GPRS_SENTENCE_SIZE + 4 + 4));
//#endif
//            bool validGPSFound = false;
            while(offset < dataSize)
            {
//                    int32_t frameNumber = *((int32_t*)&strd[offset]);
                //offset += 4;

                int32_t packetSize = strd[offset];
                offset += 1;

                char* buffer = (char*)&strd[offset];
                if(strncmp(buffer,"$GPRMC",6) != 0) {
                    break;
                }

                FMParseGPS::ParseRMC(buffer, &nmea[index],packetSize);
                char deb[1024] = {0,};
                strncpy(deb,buffer,packetSize);
                // qInfo() << index << ":" << deb << " =" << QString().sprintf("%X",offset);
                //_ParseRMC(buffer,index,NMEA_TOKEN_SIZE);
//#if (GPRMC_VALID_CHECK)
//                if( validGPSFound == false &&
//                    _gpsData[index].nStatus == 1 &&
//                    IS_VALID_LOCATION(_gpsData[index].Longitude,_gpsData[index].Latitude) == true)
//#else
//                if( validGPSFound == false && IS_VALID_LOCATION(_gpsData[index].Longitude,_gpsData[index].Latitude) == true)
//#endif
//                {
//                    validGPSFound = true;
//                }

                offset +=packetSize;
//                _bGPSExist = validGPSFound;
                index++;
            }
            gpsDone = true;
            _gps0Count = index;

        }
        else if(flag == SENS_FLAG) // Sensor Data
        {
            dataSize -= 8;  // size + flag = 8 byte
            _gsenCount = dataSize  / SENS_PACKET_SIZE;

            if(_sens != NULL)
            {
                free(_sens);
            }
            float* sens = (float*)malloc(SENS_PACKET_SIZE * _gsenCount);
            _sens = sens;

            //printf("----------------------------------------------------------\n");
            //printf("sensor data:%d\n",_sensorCount);
            //printf("----------------------------------------------------------\n");
#if (DEBUG_SENSOR_DATA)
            printf("sensor data size:%d count:%d test:%f\n",dataSize,_sensorCount,(float)dataSize / (float)(SENS_PACKET_SIZE));
#endif
            while(index < _gsenCount)
            {
                memcpy(&sens[index * NUM_SENSOR_DATA],&strd[offset],SENS_PACKET_SIZE);
                offset += SENS_PACKET_SIZE;
                //qInfo() << QString().sprintf("%.2f,%.2f,%.2f", sens[(index * NUM_SENSOR_DATA)+0],sens[(index * NUM_SENSOR_DATA)+1],sens[(index * NUM_SENSOR_DATA)+2]);
                //printf("%d,%.5f,%.5f,%.5f\n",index, _sensorData[(index * NUM_SENSOR_DATA)+0],_sensorData[(index * NUM_SENSOR_DATA)+1],_sensorData[(index * NUM_SENSOR_DATA)+2]);
#if (DEBUG_SENSOR_DATA)
                printf("idx:%d x:%.5f y:%.5f z:%.5f\n",index, _sensorData[(index * NUM_SENSOR_DATA)+0],_sensorData[(index * NUM_SENSOR_DATA)+1],_sensorData[(index * NUM_SENSOR_DATA)+2]);
#endif
                index++;
            }
            sensorDone = true;
        }
        else
        {
            break;  // flag 가 아닐경우
        }

        if(gpsDone == true && sensorDone == true)
        {
            break;
        }

        offset = nextOffset;// dataSize;  // 어차피 같아야함..
    }
    return true;
}

bool MOVFormat::parse_sensor()
{
    init_parser();

    // 1. udat
    // user 데이터 확인 완료
    set_tags("udat",NULL);
    // 탐색 시작
    parse_all();
    if(_stop_parse == false)
    {
        return false;
    }
    unsigned int udat_offset = _tag_offset_list[0];
    long udat_size = _tag_size_list[0];
    if(udat_offset != 0 && udat_size != 0)
    {
        //qInfo() << QString().sprintf("%X,%d",udat_offset+8,udat_size-8);
        RMfseek(_file,udat_offset+8,SEEK_SET);
        uint8_t *buffer = (uint8_t*)malloc(udat_size-8);
        RMfread(buffer,udat_size-8,1,_file);

        parse_buffer(buffer,udat_size-8);



        return true;
    }
    return false;
}
#endif