first commit

project/fm_viewer/data/rm_format_mov_emtudat.cpp

@@ -0,0 +1,179 @@
+#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