1363 lines
40 KiB
C++
1363 lines
40 KiB
C++
#include "rm_sensordata.h"
|
|
#include "rm_format_avi.h"
|
|
|
|
#if (FILE_FORMAT_MOV)
|
|
#include "rm_format_mov.h"
|
|
#endif
|
|
|
|
#include <string.h>
|
|
#include <stdio.h>
|
|
#include <assert.h>
|
|
|
|
// 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<char*>* 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<char*>::iterator iter = subTitles.begin(); iter != subTitles.end(); ++iter)
|
|
for(int i=0;i<subTitles->size();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<srcIndex;i++)
|
|
{
|
|
memcpy(&_gpsData[i],&_gpsData[srcIndex],sizeof(_NMEA_INFO));
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
void RMSensorData::clipGPSCount(int number)
|
|
{
|
|
if(number > 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;i<count;i++) {
|
|
// _gpsData[i].nYear = src[i].year;
|
|
// _gpsData[i].nMonth = src[i].mon;
|
|
// _gpsData[i].nDay = src[i].mday;
|
|
// _gpsData[i].nHour = src[i].hour;
|
|
// _gpsData[i].nMin = src[i].min;
|
|
// _gpsData[i].nSec = src[i].sec;
|
|
|
|
// _gpsData[i].Latitude = (double)(src[i].latitude) / 1000000.0;
|
|
// _gpsData[i].Longitude = (double)(src[i].longitude) / 1000000.0;
|
|
// _gpsData[i].Speed = src[i].gps_speed;// speed;
|
|
// _gpsData[i].nStatus = src[i].gps_speed == 0xFFFF ? 0 : 1; // gps_speed 가 0xFFFF 일 경우 invalid
|
|
// //qInfo() << _gpsData[i].Latitude << _gpsData[i].Longitude << __FUNCTION__;
|
|
// // 센서 데이터
|
|
// const int so = i*10*NUM_SENSOR_DATA;
|
|
// for(int s=0; s<10; s++) {
|
|
// //int iv = ((unsigned char*)src[i].gx[s])[1] << ((unsigned char*)src[i].gx[s])[0];
|
|
// //qInfo() << QString().sprintf("%X_%X_%X",src[i].gx[s],(int)src[i].gy[s],(int)src[i].gz[s]) << __FUNCTION__;
|
|
// const float x = (float)(*((short*)&src[i].gx[s]));
|
|
// const float y = (float)(*((short*)&src[i].gy[s]));
|
|
// const float z = (float)(*((short*)&src[i].gz[s]));
|
|
// //qInfo() << x << y << z << __FUNCTION__;
|
|
// _sensorData[so + (s * NUM_SENSOR_DATA) + 0] = qMin(qMax(x / 100.0f,-1.0f),1.0f);
|
|
// _sensorData[so + (s * NUM_SENSOR_DATA) + 1] = qMin(qMax(y / 100.0f,-1.0f),1.0f);
|
|
// _sensorData[so + (s * NUM_SENSOR_DATA) + 2] = qMin(qMax(z / 100.0f,-1.0f),1.0f);
|
|
// //qInfo() << _sensorData[so + (s * NUM_SENSOR_DATA) + 0] << _sensorData[so + (s * NUM_SENSOR_DATA) + 1] << _sensorData[so + (s * NUM_SENSOR_DATA) + 0] << __FUNCTION__;
|
|
// }
|
|
// }
|
|
// return false;
|
|
// return true;
|
|
//}
|
|
#elif(RM_MODEL == RM_MODEL_TYPE_MH9000)
|
|
bool RMSensorData::loadMOV(FILE* f)
|
|
{
|
|
MOVFormat* mov = new MOVFormat(f,VideoReadSensor);
|
|
|
|
void* sensor = NULL;
|
|
int sensorCount = mov->getSensor((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;i<count;i++) {
|
|
_gpsData[i].nYear = src[i].year;
|
|
_gpsData[i].nMonth = src[i].mon;
|
|
_gpsData[i].nDay = src[i].mday;
|
|
_gpsData[i].nHour = src[i].hour;
|
|
_gpsData[i].nMin = src[i].min;
|
|
_gpsData[i].nSec = src[i].sec;
|
|
|
|
_gpsData[i].Latitude = (double)(src[i].latitude) / 1000000.0;
|
|
_gpsData[i].Longitude = (double)(src[i].longitude) / 1000000.0;
|
|
_gpsData[i].Speed = src[i].gps_speed;// speed;
|
|
_gpsData[i].nStatus = src[i].gps_speed == 0xFFFF ? 0 : 1; // gps_speed 가 0xFFFF 일 경우 invalid
|
|
//qInfo() << _gpsData[i].Latitude << _gpsData[i].Longitude << __FUNCTION__;
|
|
// 센서 데이터
|
|
const int so = i*10*NUM_SENSOR_DATA;
|
|
for(int s=0; s<10; s++) {
|
|
//int iv = ((unsigned char*)src[i].gx[s])[1] << ((unsigned char*)src[i].gx[s])[0];
|
|
//qInfo() << QString().sprintf("%X_%X_%X",src[i].gx[s],(int)src[i].gy[s],(int)src[i].gz[s]) << __FUNCTION__;
|
|
const float x = (float)(*((short*)&src[i].gx[s]));
|
|
const float y = (float)(*((short*)&src[i].gy[s]));
|
|
const float z = (float)(*((short*)&src[i].gz[s]));
|
|
//qInfo() << x << y << z << __FUNCTION__;
|
|
_sensorData[so + (s * NUM_SENSOR_DATA) + 0] = qMin(qMax(x / 100.0f,-1.0f),1.0f);
|
|
_sensorData[so + (s * NUM_SENSOR_DATA) + 1] = qMin(qMax(y / 100.0f,-1.0f),1.0f);
|
|
_sensorData[so + (s * NUM_SENSOR_DATA) + 2] = qMin(qMax(z / 100.0f,-1.0f),1.0f);
|
|
//qInfo() << _sensorData[so + (s * NUM_SENSOR_DATA) + 0] << _sensorData[so + (s * NUM_SENSOR_DATA) + 1] << _sensorData[so + (s * NUM_SENSOR_DATA) + 0] << __FUNCTION__;
|
|
}
|
|
}
|
|
return false;
|
|
}
|
|
#elif (RM_MODEL == RM_MODEL_TYPE_ADT_CAPS && !SUB_MODEL_CARROT_EMT)
|
|
bool RMSensorData::loadMOV(FILE* f)
|
|
{
|
|
MOVFormat* mov = new MOVFormat(f,VideoReadSensor);
|
|
|
|
NMEA_INFO* gps = NULL;
|
|
SEN* sensor = NULL;
|
|
int gpsCount = mov->getGPS((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<eidx;i++) {
|
|
// space -' '0km/h
|
|
if( string[eidx-i] == ' ') {
|
|
sidx = (eidx - i);
|
|
break;
|
|
}
|
|
}
|
|
if(sidx != -1) {
|
|
return sidx;
|
|
}
|
|
}
|
|
return -1;
|
|
}
|
|
bool IsDate(char* item,NMEA_INFO* nmea)
|
|
{
|
|
// 2017-03-07
|
|
if(strlen(item) != 10 ||
|
|
item[4] != '-' ||
|
|
item[7] != '-')
|
|
{
|
|
return false;
|
|
}
|
|
|
|
for(int i=0;i<10;i++)
|
|
{
|
|
if(item[i] != '-' && (item[i] < '0' || item[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<len;i++)
|
|
{
|
|
if(item[i] != '.' && item[i] != '-' && (item[i] < '0' || item[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;
|
|
}
|