#include "rm_sensordata.h" #include "rm_format_avi.h" #if (FILE_FORMAT_MOV) #include "rm_format_mov.h" #endif #include #include #include // GPS, SENSOR TYPEDEF typedef struct _GPSINFOCHUCKTIME { unsigned char byYear; // Years since 2000 unsigned char byMon; // Months since January - [0,11] unsigned char byDay; // Day of the month - [1,31] unsigned char byHour; // Hours since midnight - [0,23] unsigned char byMin; // Minutes after the hour - [0,59] unsigned char bySec; // Seconds after the minute - [0,59] }GPSINFOCHUCKTIME; typedef struct _GPSINFOCHUCK { double dwLat; // Latitude in NDEG - +/-[degree][min].[sec/60] double dwLon; // Longitude in NDEG - +/-[degree][min].[sec/60] int32_t lAlt; // Altitude in meter +-:under/below sea level uint16_t usSpeed; // Speed unit: km/h GPSINFOCHUCKTIME sUTC; // UTC of position unsigned char ubDirection; // Clockwise degree from the North. unsigned char ubFlag; // Check if the GPS data is valid; unsigned char ubVersion; // Stuture Version unsigned char ubReserved; }GPSINFOCHUCK, *pGPSInfoChuck; char* ParserGetItem(char* string, char splitor, char* item); #define ITEM_BUFFER_LENGTH 255 bool IsTime(char* item,NMEA_INFO* nmea); bool IsDate(char* item,NMEA_INFO* nmea); long ParserIndexOf(char* string,const char find); long ParserIndexOf(char* string,const char* find); bool IsXYZ(char* string,float* xyz,float* temperature); bool IsGPS(char* item,NMEA_INFO* nmea); bool ParserIsNumeric(char* item); RMSensorData::RMSensorData(RMfile f,FILE_TYPE type) { modelName = ""; #if !(RM_MODEL_EMT_KR) _gpsCount = 0; _gpsData = NULL; #endif // #if (RM_MODEL_EMT_KR) _sensorCount = 0; _sensorData = NULL; _bGPSExist = false; #if (USE_TRIGGER) triggerE = -1.0f; triggerM = -1.0f; #endif // USE_TRIGGER #if (MODEL_BBVIEWER && !MODEL_WATEX) _OBDSpeed = NULL; #endif bool loaded = false; #if (FILE_FORMAT_AVI) if(type == FILE_TYPE_AVI) { loadAVIRiff(f); loaded = true; } #endif // FILE_FORMAT_AVI #if (FILE_FORMAT_MOV) if(!loaded && type == FILE_TYPE_MOV) { loadMOV(f); } #endif // #if (FILE_FORMAT_MOV) } RMSensorData::~RMSensorData() { #if !(RM_MODEL_EMT_KR) if(_gpsData != NULL) { free(_gpsData); _gpsData = NULL; } #endif // #if !(RM_MODEL_EMT_KR) if(_sensorData != NULL) { free(_sensorData); _sensorData = NULL; } #if (MODEL_BBVIEWER && !MODEL_WATEX) if(_OBDSpeed != NULL) { free(_OBDSpeed); _OBDSpeed = NULL; } #endif } // BBEXTRACT #if !defined(GSEN_SIZE) #define GSEN_SIZE 14 #endif #if (FILE_FORMAT_AVI && AVI_CHUNHO_SENSOR_FORMAT_1) bool RMSensorData::loadAVIChunck(uint8_t* gps, size_t gps_size,uint8_t* sensor, size_t sensor_size) { #if (CLIP_SENSOR_DATA) _skipSensorCount = 0; bool bSensorStarted = false; #endif _gpsCount = (uint32_t)gps_size / sizeof(_GPSINFOCHUCK); _sensorCount = (uint32_t)sensor_size / GSEN_SIZE; // sizeof(_GSNRINFOCHUCK) // qInfo() << "_gpsCount:" << _gpsCount; // qInfo() << "_sensorCount:" << _sensorCount; // GPS Valid 체크는 지도 그릴때 적용함 if(gps != NULL && _gpsCount > 0) { _bGPSExist = true; _gpsData = (NMEA_INFO*)malloc(sizeof(_NMEA_INFO) * _gpsCount); int nValidCount = 0; for(uint32_t i=0;i<_gpsCount;i++) { GPSINFOCHUCK* pGPS = (GPSINFOCHUCK*)&gps[i*sizeof(_GPSINFOCHUCK)]; NMEA_INFO* pDest = &_gpsData[i]; pDest->Latitude = pGPS->dwLat; pDest->Longitude = pGPS->dwLon; pDest->nYear = pGPS->sUTC.byYear + 2000; // 19 pDest->nMonth = pGPS->sUTC.byMon; // 1~12 pDest->nDay = pGPS->sUTC.byDay; pDest->nHour = pGPS->sUTC.byHour; pDest->nMin = pGPS->sUTC.byMin; pDest->nSec = pGPS->sUTC.bySec; pDest->Speed = pGPS->usSpeed; pDest->nStatus = pGPS->ubFlag; pDest->nAngle = pGPS->ubDirection; if(pDest->nStatus == 1) { nValidCount++; } // qInfo() << "GPS:" << pGPS->dwLat << " LON:" << pGPS->dwLon << " pGPS->usSpeed:" << pGPS->usSpeed << pDest->nStatus; //#if !defined(BBEXTRACT) // +/-[degree][min].[sec/60] long d = (long)(pDest->Latitude / 100); pDest->Latitude = ((double)d) + ((double)(pDest->Latitude - (d * 100)) / 60.0); d = (long)(pDest->Longitude / 100); pDest->Longitude = ((double)d) + ((double)(pDest->Longitude - (d * 100)) / 60.0); //#endif } #if !defined(BBEXTRACT) if(nValidCount == 0) { _bGPSExist = false; free(_gpsData); _gpsData = NULL; } #endif } if(sensor != NULL && _sensorCount > 0) { _sensorData = (float*)malloc(SENS_PACKET_SIZE * _sensorCount); for(uint32_t i=0;i<_sensorCount;i++) { // 범위가 -2~+2 // #166 2019-09-17 다시 1배로 변경 _sensorData[i*NUM_SENSOR_DATA+0] = (float)((double)((signed char)sensor[i*GSEN_SIZE+0]) / 128.0 * 1.0); _sensorData[i*NUM_SENSOR_DATA+1] = (float)((double)((signed char)sensor[i*GSEN_SIZE+1]) / 128.0 * 1.0); _sensorData[i*NUM_SENSOR_DATA+2] = (float)((double)((signed char)sensor[i*GSEN_SIZE+2]) / 128.0 * 1.0); //qInfo() << "XYZ(" << i << "):" << (signed char)sensor[i*GSEN_SIZE+0] << ":" << (signed char)sensor[i*GSEN_SIZE+1] << ":" << (signed char)sensor[i*GSEN_SIZE+2]; //qInfo() << i << "::: XYZ:" << _sensorData[i*NUM_SENSOR_DATA+0] << ":" << _sensorData[i*NUM_SENSOR_DATA+1] << ":" << _sensorData[i*NUM_SENSOR_DATA+2]; } } #if (CLIP_SENSOR_DATA) //qInfo() << "READ REMOVE"; if(_skipSensorCount > 0) { // SENSOR 노이즈에 따른 GPS 도 제거 clipSensorCount(_skipSensorCount); clipGPSCount(_skipSensorCount/10); } #endif //qInfo() << "_skipSensorCount:" << _skipSensorCount; return true; } #endif // #if (AVI_CHUNHO_SENSOR_FORMAT_1) #if (FILE_FORMAT_AVI && SENSOR_AVI_SUBTITLE) bool RMSensorData::loadAVISubTitle(QList* subTitles) { if (subTitles == NULL) { _gpsCount = 0; _sensorCount = 0; return false; } _gpsCount = (uint32_t)subTitles->size(); _sensorCount = (uint32_t)subTitles->size(); _sensorData = (float*)malloc(SENS_PACKET_SIZE * _sensorCount); _gpsData = (NMEA_INFO*)malloc(sizeof(_NMEA_INFO) * _gpsCount); #if defined(SHOW_DEVICE_INFO) _temperature = (float*)malloc(sizeof(float) * _sensorCount); #elif defined(SHOW_DEVICE_INFO_DETAIL) _temperatureCount = _sensorCount; _voltageCount = _sensorCount; _temperature = (uint16_t*)malloc(sizeof(uint16_t) * _temperatureCount); _voltage = (uint16_t*)malloc(sizeof(uint16_t) * _voltageCount); #endif int idx = 0; static char item[ITEM_BUFFER_LENGTH] = {0,}; static char buffer[ITEM_BUFFER_LENGTH] = {0,}; //for (std::vector::iterator iter = subTitles.begin(); iter != subTitles.end(); ++iter) for(int i=0;isize();i++) { memset(item,0,ITEM_BUFFER_LENGTH); memset(buffer,0,ITEM_BUFFER_LENGTH); strcpy(buffer,subTitles->at(i)); char* string = &buffer[0]; //qInfo () << i << string; #if (APPLICATION_LOG) // qInfo () << string; #endif NMEA_INFO* nmea = &_gpsData[idx]; float* xyz = &_sensorData[NUM_SENSOR_DATA * idx]; //#if defined(SHOW_DEVICE_INFO) // float* temperature = &_temperature[idx]; //#elif defined(SHOW_DEVICE_INFO_DETAIL) // uint16_t* temperature = &_temperature[idx]; // uint16_t* voltage = &_voltage[idx]; //#else float temperatureDummy; float* temperature = &temperatureDummy; //#endif // 1. model name string = ParserGetItem(string,':',item); if(strlen(item) > 0) { _modelName = QString(item); //strcpy(_modelName,item); } // 2. date string = ParserGetItem(string,' ',item); if(IsDate(item,nmea)) { } // 3. time string = ParserGetItem(string,' ',item); if(IsTime(item,nmea)) { } // 4. KeyString or Sensor X if(ParserIndexOf(string,"X:") > 0) // KeyString1,2 Exist { string = ParserGetItem(string,' ',item); // strcpy(_keyString1,item); if(ParserIndexOf(string,"X:") > 0) // KeyString2 { string = ParserGetItem(string,' ',item); //strcpy(_keyString2,item); } } // 5. Sensor XYZ + temperature string = ParserGetItem(string,'V',item); // volt #if defined(SHOW_DEVICE_INFO_DETAIL) if(IsXYZ(item,xyz,temperature,voltage)) { } #else if(IsXYZ(item,xyz,temperature)) { } #endif // 6. GPS // G 37.487434 N 127.021271 E 0.1km/h string = ParserGetItem(string,'/',item); // km / h if(IsGPS(item,nmea)) { _bGPSExist = true; nmea->nStatus = 1; } idx++; } //qInfo() << "count:" << idx << "gps:" << _bGPSExist << __FUNCTION__; return true; } #endif // SENSOR_AVI_SUBTITLE #if (CLIP_SENSOR_DATA) // 초기 센서값이 망가진 데이터의 GPS 데이터 // 의 정상 데이터를 탐색하여 5초내 존재하며 시속 20km 이상일 경우 // 복사하여 복구 void RMSensorData::fixGPS() { // qInfo() << "_skipSensorCount:" << _skipSensorCount << " _gpsCount:" << _gpsCount << " status:" << _gpsData[0].nStatus; // _skipSensorCount > 1 && 2019/09/26 제거 if(_gpsCount > 5 && _gpsData != NULL && _gpsData[0].nStatus == 0) { int srcIndex = -1; for(int i=0;i<5;i++) { // found if(_gpsData[i].nStatus == 1 && _gpsData[i].Speed > 20) { srcIndex = i; break; } } // 이동보다는 복제가 타이밍이 더나음.. // 1개 샘플은 그저.. if(srcIndex > 0) { // clipGPSCount(srcIndex); // qInfo() << "clip:" << srcIndex << " _gpsData[i].Speed:" << _gpsData[srcIndex].Speed; for(int i=0;i 0 && _gpsCount > (unsigned int)number && _gpsData != NULL) { //qInfo() << "remove gps:" << number; size_t nsize = sizeof(_NMEA_INFO) * (_gpsCount - number); NMEA_INFO* ns = (NMEA_INFO*)malloc(nsize); memcpy(ns,&_gpsData[number],nsize); _gpsCount -= number; free(_gpsData); _gpsData = ns; } } void RMSensorData::clipSensorCount(int number) { if(number > 0 && _sensorCount > (unsigned int)number && _sensorData != NULL) { //qInfo() << "remove sensor:" << number; size_t nsize = SENS_PACKET_SIZE * (_sensorCount - number); float* ns = (float*)malloc(nsize); memcpy(ns,&_sensorData[number*NUM_SENSOR_DATA],nsize); _sensorCount -= number; free(_sensorData); _sensorData = ns; } } void RMSensorData::clipWithDuration(qint64 duration) { //qInfo() << "DURATION REMOVE" << duration << _sensorCount << _gpsCount << "T:" << duration / 10000; int nSkip = _sensorCount - (int)(duration / 100); if(nSkip > 0) { // 전방 클립하면 안됨 _sensorCount = (int)(duration / 100); //clipSensorCount(nSkip); } nSkip = _gpsCount - (int)(duration / 1000); if(nSkip > 0) { _gpsCount = (int)(duration / 1000); //clipGPSCount(nSkip); } } #endif #if (FILE_FORMAT_AVI) bool RMSensorData::loadAVIRiff(RMfile f) { VideoPreInfo info = {0,}; AVIRiff* avi = new AVIRiff(f,VideoReadSensor,&info); #if (SENSOR_AVI_SUBTITLE) //qInfo() << __FUNCTION__ << avi->subtitles->size() << info.duration; loadAVISubTitle(avi->subtitles); //qInfo() << _gpsCount << _bGPSExist << __FUNCTION__; #endif #if (AVI_CHUNHO_SENSOR_FORMAT_1) uint8_t* gps = NULL; uint8_t* sensor = NULL; size_t gps_size = 0; size_t sensor_size = 0; avi->getChunckData(&gps,&gps_size,&sensor,&sensor_size); loadAVIChunck(gps,gps_size,sensor,sensor_size); #endif delete avi; #if defined(BBEXTRACT) fseek(f,0,SEEK_SET); VideoPreInfo info = {0,}; AVIRiff::duration(f,&info); durationInMSec = info.duration; #endif return true; } #endif // #if (FILE_FORMAT_AVI) #if (FILE_FORMAT_MOV) #if (RM_MODEL == RM_MODEL_TYPE_NX_DRW22) bool RMSensorData::loadMOV(FILE* f) { MOVFormat* mov = new MOVFormat(f,VideoReadSensor); GPS0* gps = NULL; int16_t* sensor = NULL; int gpsCount = mov->getGPS(&gps); int sensorCount = mov->getSensor(&sensor); // GPSNR* gpsnr = NULL; // int count = mov->getGPSNR(&gpsnr); loadMOVGPSNR(gps,gpsCount,sensor,sensorCount); #if defined(BBEXTRACT) fseek(f,0,SEEK_SET); VideoPreInfo info = {0,}; MOVFormat::duration(f,&info); durationInMSec = info.duration; #endif delete mov; return true; } bool RMSensorData::loadMOVGPSNR(void* p,uint32_t count, void* ps, uint32_t sensorCount) { GPS0* gps = (GPS0*)p; int16_t* sensor = (int16_t*)ps; #if (CLIP_SENSOR_DATA) _skipSensorCount = 0; //bool bSensorStarted = false; #endif _gpsCount = count; _sensorCount = sensorCount; // GPS Valid 체크는 지도 그릴때 적용함 if(gps == NULL || count == 0) { _gpsCount = 0; _sensorCount = 0; return false; } _bGPSExist = true; _gpsData = (NMEA_INFO*)malloc(sizeof(_NMEA_INFO) * _gpsCount); #if defined(BBEXTRACT) _sensorData = (GSENSOR*)malloc(sizeof(_GSENSOR) * _sensorCount); #else _sensorData = (float*)malloc(SENS_PACKET_SIZE * _sensorCount); #endif #if (MODEL_BBVIEWER) _OBDSpeed = (float*)malloc(sizeof(float) * _sensorCount); #endif int nValidGPSCount = 0; for(uint32_t i=0;i<_gpsCount;i++) { GPS0* src = &gps[i]; //GPSINFOCHUCK* pGPS = (GPSINFOCHUCK*)&gps[i*sizeof(_GPSINFOCHUCK)]; NMEA_INFO* pDest = &_gpsData[i]; pDest->Latitude = src->lat; pDest->Longitude = src->lon; pDest->nYear = src->year + 2000; // 19 pDest->nMonth = src->month; // 1~12 pDest->nDay = src->day; pDest->nHour = src->hour; pDest->nMin = src->min; pDest->nSec = src->sec; pDest->Speed = src->speed; pDest->nStatus = src->status; pDest->nAngle = src->degree; if(pDest->nStatus == 1) { nValidGPSCount++; } long d = (long)(pDest->Latitude / 100); pDest->Latitude = ((double)d) + ((double)(pDest->Latitude - (d * 100)) / 60.0); d = (long)(pDest->Longitude / 100); pDest->Longitude = ((double)d) + ((double)(pDest->Longitude - (d * 100)) / 60.0); } for(uint32_t i=0;i<_sensorCount;i++) { //int16_t* src = &sensor[i]; // SENSOR // 일단 YMD 데이터가 엉터리라 CLIP 하지 않는다 // 최대, 최소값 G const double min_max = 10.0; // ZYX -> XYZ _sensorData[i*NUM_SENSOR_DATA+0] = (float)qMin(qMax(((double)(sensor[i*NUM_SENSOR_DATA+2]) / 1000.0f),-min_max),min_max); _sensorData[i*NUM_SENSOR_DATA+1] = (float)qMin(qMax(((double)(sensor[i*NUM_SENSOR_DATA+1]) / 1000.0f),-min_max),min_max); _sensorData[i*NUM_SENSOR_DATA+2] = (float)qMin(qMax(((double)(sensor[i*NUM_SENSOR_DATA+0]) / 1000.0f),-min_max),min_max); //qInfo() << sensor[i*NUM_SENSOR_DATA+2] << "," << sensor[i*NUM_SENSOR_DATA+1] << "," << sensor[i*NUM_SENSOR_DATA+0]; //qInfo() << _sensorData[i*NUM_SENSOR_DATA+0] << _sensorData[i*NUM_SENSOR_DATA+1] << _sensorData[i*NUM_SENSOR_DATA+2]; //qInfo() << "XYZ(" << i << "):" << (signed char)sensor[i*GSEN_SIZE+0] << ":" << (signed char)sensor[i*GSEN_SIZE+1] << ":" << (signed char)sensor[i*GSEN_SIZE+2]; // if(i == 0) { // qInfo() << QString().sprintf("X:%d=%f",sensor[2],_sensorData[0]); // } } // qInfo() << __FUNCTION__ << _sensorCount; if(nValidGPSCount == 0) { _bGPSExist = false; free(_gpsData); _gpsData = NULL; } /* #if (CLIP_SENSOR_DATA) //qInfo() << "READ REMOVE"; if(_skipSensorCount > 0) { // SENSOR 노이즈에 따른 GPS 도 제거 clipSensorCount(_skipSensorCount); clipGPSCount(_skipSensorCount/10); } #endif */ //qInfo() << "_skipSensorCount:" << _skipSensorCount; return true; } #elif (RM_MODEL == RM_MODEL_TYPE_XLDR_88 || SUB_MODEL_CARROT_EMT) bool RMSensorData::loadMOV(FILE* f) { MOVFormat* mov = new MOVFormat(f,VideoReadSensor); // if(!mov->isValid()) { // delete mov; // return false; // } NMEA_INFO* nmea = NULL; _gpsCount = mov->getGPS((void**)&nmea); int nValidGPSCount = 0; _bGPSExist = true; for(uint32_t i=0;i<_gpsCount;i++) { NMEA_INFO* pDest = &nmea[i]; if(pDest->nStatus == 1) { nValidGPSCount++; } } if(nValidGPSCount == 0) { _bGPSExist = false; free(_gpsData); _gpsData = NULL; } else { _gpsData = (NMEA_INFO*)malloc(sizeof(_NMEA_INFO) * _gpsCount); memcpy(_gpsData,nmea,sizeof(_NMEA_INFO)*_gpsCount); } float* sensor; _sensorCount = mov->getSensor((void**)&sensor); if(_sensorCount > 0) { _sensorData = (float*)malloc(SENS_PACKET_SIZE * _sensorCount); memcpy(_sensorData,sensor,SENS_PACKET_SIZE * _sensorCount); // for(int i=0;i<_sensorCount;i++) { // _sensorData[i*NUM_SENSOR_DATA+1] -= 3.0f; // } } return true; } #elif (RM_MODEL == RM_MODEL_TYPE_KEIYO1 || RM_MODEL == RM_MODEL_TYPE_MBJ5010 || RM_MODEL == RM_MODEL_TYPE_FC_DR232W || RM_MODEL == RM_MODEL_TYPE_BV2000) bool RMSensorData::loadMOV(FILE* f) { MOVFormat* mov = new MOVFormat(f,VideoReadSensor); NMEA_INFO* gps = NULL; float* sensor = NULL; int gpsCount = mov->getGPS((void**)&gps); int sensorCount = mov->getSensor((void**)&sensor); _sensorFPS = mov->getSensorFPS(); loadMOVGPSNR(gps,gpsCount,sensor,sensorCount); delete mov; return false; } void RMSensorData::processWithDuration(qint64 ms) { // GPS 데이터 if(_gpsCount > 0 && _gpsData != NULL) { // 짧으면 늘리기 uint32_t gpsCount = ms / 1000; if(_gpsCount < gpsCount) { NMEA_INFO* temp = (NMEA_INFO*)malloc(sizeof(_NMEA_INFO) * gpsCount); memset(temp,0,sizeof(_NMEA_INFO) * gpsCount); memcpy(temp,_gpsData,sizeof(_NMEA_INFO)*_gpsCount); //qInfo() << "GPS EXTEND" << _gpsCount << gpsCount << LOG_FL; _gpsCount = gpsCount; free(_gpsData); _gpsData = temp; } else if (_gpsCount > gpsCount) { // 길면 줄이기 //qInfo() << "GPS CLIP" << _gpsCount << gpsCount << LOG_FL; _gpsCount = gpsCount; } } // 센서 데이터 if (_sensorCount > 0 && _sensorData != NULL) { uint32_t sensorCount = (ms / 1000) * _sensorFPS; if(_sensorCount < sensorCount) { float* temp = (float*)malloc(SENS_PACKET_SIZE * sensorCount); memset(temp,0,SENS_PACKET_SIZE * sensorCount); memcpy(temp,_sensorData,SENS_PACKET_SIZE * _sensorCount); //qInfo() << "SENSOR EXTEND" << _sensorCount << sensorCount << LOG_FL; _sensorCount = sensorCount; free(_sensorData); _sensorData = temp; } else if (_sensorCount > sensorCount) { // 길면 줄이기 //qInfo() << "SENSOR CLIP" << _sensorCount << sensorCount << LOG_FL; _sensorCount = sensorCount; } } } bool RMSensorData::loadMOVGPSNR(void* p,uint32_t count, void* ps, uint32_t sensorCount) { NMEA_INFO* gps = (NMEA_INFO*)p; float* sensor = (float*)ps; _gpsCount = count; _sensorCount = sensorCount; // GPS Valid 체크는 지도 그릴때 적용함 if(gps == NULL || count == 0) { _gpsCount = 0; _sensorCount = 0; return false; } _bGPSExist = true; _gpsData = (NMEA_INFO*)malloc(sizeof(_NMEA_INFO) * _gpsCount); memcpy(_gpsData,gps,sizeof(_NMEA_INFO)*_gpsCount); _sensorData = (float*)malloc(SENS_PACKET_SIZE * _sensorCount); int nValidGPSCount = 0; for(uint32_t i=0;i<_gpsCount;i++) { NMEA_INFO* pDest = &_gpsData[i]; if(pDest->nStatus == 1) { nValidGPSCount++; } } for(uint32_t i=0;i<_sensorCount;i++) { float* src = &sensor[i*NUM_SENSOR_DATA]; //qInfo() << src->x << src->y << src->z << ":" << src->gcal; // SENSOR // 일단 YMD 데이터가 엉터리라 CLIP 하지 않는다 _sensorData[i*NUM_SENSOR_DATA+0] = (float)qMin(qMax(src[0],-1.0f),1.0f); // X _sensorData[i*NUM_SENSOR_DATA+1] = (float)qMin(qMax(src[1],-1.0f),1.0f); // Y _sensorData[i*NUM_SENSOR_DATA+2] = (float)qMin(qMax(src[2],-1.0f),1.0f); // Z } if(nValidGPSCount == 0) { _bGPSExist = false; free(_gpsData); _gpsData = NULL; } return false; } #elif (RM_MODEL == RM_MODEL_TYPE_EMT_KR) void RMSensorData::processWithDuration(qint64 ms) { Q_UNUSED(ms) } bool RMSensorData::loadMOV(FILE* f) { MOVFormat* mov = new MOVFormat(f,VideoReadSensor); void* sensor = NULL; _sensorCount = mov->getNMEA((void**)&sensor); #if (RM_MODEL_EMT_KR) modelName = mov->modelName; #endif // #if (RM_MODEL_EMT_KR) _bGPSExist = true; _sensorData = (NMEA_INFO*)malloc(sizeof(_NMEA_INFO) * _sensorCount); memcpy(_sensorData,sensor,sizeof(_NMEA_INFO) * _sensorCount); for(uint32_t i=0;i<_sensorCount;i++) { #if (USE_TRIGGER) if(_sensorData[i].eTrigger != 255) { triggerE = (float)i / (float)_sensorCount; } if(_sensorData[i].mTrigger != 255) { triggerM = (float)i / (float)_sensorCount; } #endif // USE_TRIGGER // _sensorData[i].x = ((float)(rand() % 100) / 100.0f) * 2.0f - 1.0f; // _sensorData[i].y = ((float)(rand() % 100) / 100.0f) * 2.0f - 1.0f; // _sensorData[i].z = ((float)(rand() % 100) / 100.0f) * 2.0f - 1.0f; // qInfo() << "ll:" << _sensorData[i].Latitude << _sensorData[i].Longitude << __FUNCTION__; } // if(triggerE < 0.0f) { // triggerE = 0.3333; // } // if(triggerM < 0.0f) { // triggerM = 0.6666; // } //loadMOVGPSNR(sensor,sensorCount); delete mov; return true; } //bool RMSensorData::loadMOVGPSNR(void* p,uint32_t count) //{ // NMEA_INFO* src = (NMEA_INFO*)p; // _gpsCount = count; // _sensorCount = count * 10; // _bGPSExist = true; // _gpsData = (NMEA_INFO*)malloc(sizeof(_NMEA_INFO) * _gpsCount); // _sensorData = (float*)malloc(SENS_PACKET_SIZE * _sensorCount); // for(uint32_t i=0;igetSensor((void**)&sensor); _sensorFPS = 10; loadMOVGPSNR(sensor,sensorCount); delete mov; return false; } void RMSensorData::processWithDuration(qint64 ms) { // GPS 데이터 if(_gpsCount > 0 && _gpsData != NULL) { // 짧으면 늘리기 uint32_t gpsCount = ms / 1000; if(_gpsCount < gpsCount) { NMEA_INFO* temp = (NMEA_INFO*)malloc(sizeof(_NMEA_INFO) * gpsCount); memset(temp,0,sizeof(_NMEA_INFO) * gpsCount); memcpy(temp,_gpsData,sizeof(_NMEA_INFO)*_gpsCount); //qInfo() << "GPS EXTEND" << _gpsCount << gpsCount << LOG_FL; _gpsCount = gpsCount; free(_gpsData); _gpsData = temp; } else if (_gpsCount > gpsCount) { // 길면 줄이기 //qInfo() << "GPS CLIP" << _gpsCount << gpsCount << LOG_FL; _gpsCount = gpsCount; } } // 센서 데이터 if (_sensorCount > 0 && _sensorData != NULL) { uint32_t sensorCount = (ms / 1000) * _sensorFPS; if(_sensorCount < sensorCount) { float* temp = (float*)malloc(SENS_PACKET_SIZE * sensorCount); memset(temp,0,SENS_PACKET_SIZE * sensorCount); memcpy(temp,_sensorData,SENS_PACKET_SIZE * _sensorCount); //qInfo() << "SENSOR EXTEND" << _sensorCount << sensorCount << LOG_FL; _sensorCount = sensorCount; free(_sensorData); _sensorData = temp; } else if (_sensorCount > sensorCount) { // 길면 줄이기 //qInfo() << "SENSOR CLIP" << _sensorCount << sensorCount << LOG_FL; _sensorCount = sensorCount; } } } bool RMSensorData::loadMOVGPSNR(void* p,uint32_t count) { gps_chunk_t* src = (gps_chunk_t*)p; _gpsCount = count; _sensorCount = count * 10; _bGPSExist = true; _gpsData = (NMEA_INFO*)malloc(sizeof(_NMEA_INFO) * _gpsCount); _sensorData = (float*)malloc(SENS_PACKET_SIZE * _sensorCount); for(uint32_t i=0;igetGPS((void**)&gps); int sensorCount = mov->getSensor((void**)&sensor); loadMOVGPSNR(gps,gpsCount,sensor,sensorCount); delete mov; return true; } bool RMSensorData::loadMOVGPSNR(void* p,uint32_t count, void* ps, uint32_t sensorCount) { NMEA_INFO* gps = (NMEA_INFO*)p; SEN* sensor = (SEN*)ps; _gpsCount = count; _sensorCount = sensorCount; // GPS Valid 체크는 지도 그릴때 적용함 if(gps == NULL || count == 0) { _gpsCount = 0; _sensorCount = 0; return false; } _bGPSExist = true; _gpsData = (NMEA_INFO*)malloc(sizeof(_NMEA_INFO) * _gpsCount); memcpy(_gpsData,gps,sizeof(_NMEA_INFO)*_gpsCount); _sensorData = (float*)malloc(SENS_PACKET_SIZE * _sensorCount); int nValidGPSCount = 0; for(uint32_t i=0;i<_gpsCount;i++) { NMEA_INFO* pDest = &_gpsData[i]; if(pDest->nStatus == 1) { nValidGPSCount++; } } for(uint32_t i=0;i<_sensorCount;i++) { SEN* src = &sensor[i]; //qInfo() << src->x << src->y << src->z << ":" << src->gcal; // SENSOR // 일단 YMD 데이터가 엉터리라 CLIP 하지 않는다 _sensorData[i*NUM_SENSOR_DATA+0] = (float)qMin(qMax(src->x,-1.0f),1.0f); _sensorData[i*NUM_SENSOR_DATA+1] = (float)qMin(qMax(src->y,-1.0f),1.0f); _sensorData[i*NUM_SENSOR_DATA+2] = (float)qMin(qMax(src->z,-1.0f),1.0f); } if(nValidGPSCount == 0) { _bGPSExist = false; free(_gpsData); _gpsData = NULL; } } #elif (RM_MODEL == RM_MODEL_TYPE_TB4000) bool RMSensorData::loadMOV(FILE* f) { MOVFormat* mov = new MOVFormat(f,VideoReadSensor); GPSINFOCHUCK_TELEBIT* gps = NULL; int gpsCount = mov->getGPS(&gps); loadMOVGPSNR(gps,gpsCount,NULL,0); delete mov; return true; } bool RMSensorData::loadMOVGPSNR(void* p,uint32_t count, void* ps, uint32_t sensorCount) { GPSINFOCHUCK_TELEBIT* gps = (GPSINFOCHUCK_TELEBIT*)p; _gpsCount = count; _sensorCount = 0; // GPS Valid 체크는 지도 그릴때 적용함 if(gps == NULL || count == 0) { _gpsCount = 0; _sensorCount = 0; return false; } _bGPSExist = true; _gpsData = (NMEA_INFO*)malloc(sizeof(_NMEA_INFO) * _gpsCount); //memcpy(_gpsData,gps,sizeof(_NMEA_INFO)*_gpsCount); _sensorData = NULL; int nValidGPSCount = 0; for(uint32_t i=0;i<_gpsCount;i++) { GPSINFOCHUCK_TELEBIT* pSrc = &gps[i]; NMEA_INFO* pDest = &_gpsData[i]; if(pSrc->ubFlag == 1) { nValidGPSCount++; } pDest->nStatus = pSrc->ubFlag; // // DM->DD 로 변환 double t = (((int)(pSrc->dwLat / 100.0)) * 100.0); pDest->Latitude = (t / 100.0) + ((pSrc->dwLat - t) / 60.0); t = (((int)(pSrc->dwLon / 100.0)) * 100.0); pDest->Longitude = (t / 100.0) + ((pSrc->dwLon - t) / 60.0); // qInfo() << pDest->Latitude << pSrc->dwLat << __FUNCTION__; // qInfo() << pSrc->dwLat << pSrc->dwLon << QString().sprintf("%.6f,%.6f",pDest->Latitude,pDest->Longitude) << _gpsCount << __FUNCTION__; //pDest->Latitude = pSrc->dwLat / 100.0; //pDest->Longitude = pSrc->dwLon / 100.0; } if(nValidGPSCount == 0) { _bGPSExist = false; free(_gpsData); _gpsData = NULL; } return true; } #else // DUMMY bool RMSensorData::loadMOV(FILE* f) { return false; } #endif // RM_MODEL_TYPE_NX_DRW22 #endif // #if (FILE_FORMAT_MOV) char* ParserTrimSpace(char* string) { if (string[0] != ' ' || string == NULL || string[0] == '\0') { return string; } // remove first space int len = (int)MIN(strlen(string),ITEM_BUFFER_LENGTH); while(*string == ' ' && len > 0) { string++; len--; } return string; } char* ParserGetItem(char* string, char splitor, char* item) { item[0] = '\0'; if (string == NULL || string[0] == '\0') { return NULL; } string = ParserTrimSpace(string); if (string[0] == splitor) { return string + 1; // next start } unsigned int len = (unsigned int)MIN(strlen(string),ITEM_BUFFER_LENGTH); while( *string != splitor && *string != '\0' && len > 0) { *item = *string; item++; string++; len--; } string++; *item = '\0'; return string; } long ParserIndexOf(char* string,const char* find) { if (string == NULL || string[0] == '\0') { return -1; } string = ParserTrimSpace(string); char *pch = strstr(string, find); if(pch != NULL) { return ((unsigned long)pch - (unsigned long)string); } return -1; } long ParserIndexOf(char* string,const char find) { if (string == NULL || string[0] == '\0') { return -1; } string = ParserTrimSpace(string); char *pch = strchr(string,find); if(pch != NULL) { return ((unsigned long)pch - (unsigned long)string); } return -1; } long ParserIndexWithSuffix(char* string,const char* suffix) { if (string == NULL || string[0] == '\0') { return -1; } string = ParserTrimSpace(string); char *pch = strstr(string,suffix); if(pch != NULL) { int eidx = ((unsigned long)pch - (unsigned long)string); // suffix 제외하고 문자열 int sidx = -1; for(int i=0;i '9' )) { return false; } } char date[11] = {0,}; memset(date,0,10); strcpy(date,item); date[4] = 0; date[7] = 0; nmea->nYear = atoi(&date[0]); nmea->nMonth = atoi(&date[5]); nmea->nDay = atoi(&date[8]); return true; } bool IsXYZ(char* string,float* xyz,float* temperature) { // X: 0.63 Y: 0.20 Z: 0.73 34.1T 12.0V // X: 0.07 Y: 0.02 Z: 0.96 46.0T 12.9V string = ParserTrimSpace(string); char valueX[10] = {0,}; char valueY[10] = {0,}; char valueZ[10] = {0,}; int idxs[3] = {0,}; idxs[0] = ParserIndexOf(string,"X:"); idxs[1] = ParserIndexOf(string,"Y:"); idxs[2] = ParserIndexOf(string,"Z:"); if(idxs[0] == -1 || idxs[1] == -1 || idxs[2] == -1) { return false; } ParserGetItem(&string[idxs[0]+2],' ',valueX); ParserGetItem(&string[idxs[1]+2],' ',valueY); char* t = ParserGetItem(&string[idxs[2]+2],' ',valueZ); if(ParserIsNumeric(valueX) == false || ParserIsNumeric(valueY) == false || ParserIsNumeric(valueZ) == false) { return false; } xyz[0] = (float)atof(valueX); xyz[1] = (float)atof(valueY); xyz[2] = (float)atof(valueZ); char valueT[10] = {0,}; #if defined(SHOW_DEVICE_INFO_DETAIL) char* v = ParserGetItem(t,' ',valueT); #else ParserGetItem(t,' ',valueT); #endif int idxT = ParserIndexOf(valueT,"T"); if(idxT != -1) { valueT[idxT] = '\0'; } if(ParserIsNumeric(valueT) == true) { #if defined(SHOW_DEVICE_INFO_DETAIL) *temperature = (uint16_t)(atof(valueT) * 10.0); #else *temperature = (float)atof(valueT); #endif } //qInfo() << "T:" << valueT << *temperature; #if defined(SHOW_DEVICE_INFO_DETAIL) if(ParserIsNumeric(v) == true) { *voltage = (uint16_t)(atof(v) * 10.0); } #endif return true; } bool IsTime(char* item,NMEA_INFO* nmea) { // 12:44:26 if(strlen(item) != 8 || item[2] != ':' || item[5] != ':') { return false; } for(int i=0;i<8;i++) { if(item[i] != ':' && (item[i] < '0' || item[i] > '9' )) { return false; } } static char time[10] = {0,}; memset(time,0,10); strcpy(time,item); time[2] = 0; time[5] = 0; nmea->nHour = atoi(&time[0]); nmea->nMin = atoi(&time[3]); nmea->nSec = atoi(&time[6]); return true; } bool ParserIsNumeric(char* item) { if (item == NULL || item[0] == '\0') { return false; } size_t len = strlen(item); for(size_t i=0;i '9' )) { return false; } } return true; } bool IsGPS(char* item,NMEA_INFO* nmea) { // G 37.487434 N 127.021271 E 0.1km/h // - --.------ - ---.------ - ---.-km/h // COMTEC // G 37.4849739 N 127.0021896 E 1km/h char valueLat[12] = {0,}; char valueLon[12] = {0,}; char valueSpeed[12] = {0,}; int idxs[3] = {0,}; #if (SPEED_ALWAYS_EXITS) double sn = 1.0; // south north double we = 1.0; // west east #endif // #if !(SPEED_ALWAYS_EXITS) idxs[0] = ParserIndexOf(item,'G'); // 위도 시작 idxs[1] = ParserIndexOf(item,'N'); // 경도 if(idxs[1] == -1) { idxs[1] = ParserIndexOf(item,'S'); #if (SPEED_ALWAYS_EXITS) sn = -1.0; #endif // } idxs[2] = ParserIndexOf(item,'E'); // 속도 if(idxs[2] == -1) { idxs[2] = ParserIndexOf(item,'W'); #if (SPEED_ALWAYS_EXITS) we = -1.0; #endif // } #if !(SPEED_ALWAYS_EXITS) Q_UNUSED(nmea) if(idxs[0] == -1 || idxs[1] == -1 || idxs[2] == -1) { return false; } ParserGetItem(&item[idxs[0]+1],' ',valueLat); // 위도 가져오기 ParserGetItem(&item[idxs[1]+1],' ',valueLon); // 경도 가져오기 ParserGetItem(&item[idxs[2]+1],'k',valueSpeed); #else // SPEED_ALWAYS_EXITS // 32 XLDR88:2021-01-06 10:39:31 X: 0.00 Y: 0.96 Z:-0.29 009.0T 12.4V - --.------ - ---.------ - 0km/h V:972 S:0k HDR if(idxs[0] != -1) { ParserGetItem(&item[idxs[0]+1],' ',valueLat); // 위도 가져오기 } if(idxs[1] != -1) { ParserGetItem(&item[idxs[1]+1],' ',valueLon); // 경도 가져오기 } if(idxs[2] == -1) { idxs[2] = ParserIndexWithSuffix(item,"km"); // 경위도가 없는 경우에도 suffix 로 확인 } if(idxs[2] != -1) { ParserGetItem(&item[idxs[2]+1],'k',valueSpeed); // 속도 가져오기 } #endif // SPEED_ALWAYS_EXITS #if !(SPEED_ALWAYS_EXITS) if(ParserIsNumeric(valueLat) == false || ParserIsNumeric(valueLon) == false || ParserIsNumeric(valueSpeed) == false) { return false; } #else // SPEED_ALWAYS_EXITS if(ParserIsNumeric(valueLat)) { nmea->Latitude = atof(valueLat) * sn; } if(ParserIsNumeric(valueLon)) { nmea->Longitude = atof(valueLon) * we; } if(ParserIsNumeric(valueSpeed)) { nmea->Speed = atof(valueSpeed); } #endif // SPEED_ALWAYS_EXITS return true; }