/********************************************************************/ /* FILE NAME : scene_recognition.c */ /* AUTHOR : ZhangJingrui, ID: 11082826 */ /* VERSION : V0.85_Beta (Doing) */ /* DESCRIPTION: The definations of functions in scene_recognition.h */ /********************************************************************/ #include "scene_recognition.h" #include #include /* fabs() */ #include /* malloc(), free() */ /* @Lai Zhilong added below code*/ #if !defined(LOCATION_PLATFORM_QCOM_MODEM) /* The Macro is in "custom.h" */ #include /* @Zhang Jingrui: The header is depercated (and quite Linux specific, */ /* on which it defines non-standard functions like mallinfo(3)). Notice that the */ /* header is defined by C89 (and later) standards, but not . */ #endif #include /* memset() */ #include /* FLT_EPSILON */ /* Macro Defination----------------------------------------------*/ /*===============================================================*/ /* Compatible with the value in header file "sensor.h" */ #define _CONSTELLATION_USED_CAPACITY 12 /* Size of array defined as lct_nmea_gsa::prn. */ #define _CONSTELLATION_VISIBLE_CAPACITY 20 /* Size of array defined as lct_nmea_gsv::satellites_data. */ /* Satellite PRN index in _GnssInfo list. */ /* These index are planed with _GNSS_INDEX_LEN. */ /* GP: index in _GnssInfo's list is 0~31, and the PRN index is 1~32 */ #define _GP_INDEX_L 0 #define _GP_INDEX_R 31 #define _GP_INDEX_OFFSET ( _GP_INDEX_L ) #define _GP_SVID_OFFSET 1 /* GA: index in _GnssInfo's list is 32~67, and the PRN index is 1~36 */ #define _GA_INDEX_L 32 #define _GA_INDEX_R 67 #define _GA_INDEX_OFFSET ( _GA_INDEX_L ) #define _GA_SVID_OFFSET 1 /* GL: index in _GnssInfo's list is 68~102, and the PRN index is 65~99 */ #define _GL_INDEX_L 68 #define _GL_INDEX_R 102 #define _GL_INDEX_OFFSET ( _GL_INDEX_L ) #define _GL_SVID_OFFSET 65 /* Global Variable-----------------------------------------------*/ /*===============================================================*/ /* g_gnss_info_buffer: the newnest value is in the 0th sub-stucture. */ static GnssInfo* g_gnss_info_buffer = NULL; static const int g_gnss_info_buffer_size = _TIME_BUFFER_SIZE; /* g_gnss_info_buffer_trail: 0 ~ gnss_info_buffer_size */ static int g_gnss_info_buffer_trail = 0; static _SCENE_MODEL_WORK_STATE g_model_state = MODEL_OFF; /* static function Declaration-----------------------------------*/ /*===============================================================*/ /* Return @ _is_occupied: 0 -- non resource occupied. */ static int check_resources_occupied(void); /* Return @ _is_failed: 0 -- process of free res is successful. */ static int free_resources (void); /* Return @ _is_failed: 0 -- process of allocating res is successful. */ static int alloc_resources (void); /* Function Name: static _lctnmea_to_gnssinfo() */ /* Usage: Transform from the updated lct_nmea structure to the _GnssInfo structure. */ /* The defination of lct_nmea is in . */ /* Param @ _dst : The pointer to the target _GnssInfo structure variable.*/ /* Param @ _src_nmea : The pointer to the source lct_nmea structure varible. */ /* Return @ _parse_res_state: The transformation process result. 0--success; -1--NULL param inputed; 1--lct_nmea _src_nmea is not valiable beacause of the update flag. */ static int lctnmea2Gnssinfo (GnssInfo *, const lct_nmea *); /* Function name: static _detect_good_signal() */ /* Usage: judge the quality of positioning result between good and bad. */ /* Param @ _src: The pointer to the _GnssInfo structure in the positioning epoch. */ /* The value in the structure should be avaliable. */ /* Return @ _process_res: the quality of positioning result. */ /* _SIGNAL_QUALITY::GOOD -- the signal is good. */ /* _SIGNAL_QUALITY::BAD -- the signal is not good. */ static SIGNAL_QUALITY detGnssSignal (const GnssInfo *); /* Function name: static _gnss_info_st_cpy() */ /* Usage: Deep copy from one _GnssInfo to another one. */ /* The rule of copy operation: */ /* _dst | _src | Operation behavior */ /* -------------------------------------- */ /* NULL | variable | Nothing done. */ /* NULL | NULL | Nothing done. */ /* val A | NULL | Set the value of A to the initialization value. */ /* val A | val B | Deep copy B to A. */ /* Val A | val A | Nothing done. */ /* Param @ _src: Point to the source _GnssInfo structure. */ /* Param @ _dst: Point to the target _GnssInfo structure . */ /* Return @ (void) */ static void gnss_info_st_cpy (GnssInfo *_dst, const GnssInfo *_src); /* external function defination ---------------------------------*/ /*===============================================================*/ SCENE_INIT_STATE initSceneRecognition(void) { SCENE_INIT_STATE res = INIT_NOT_PERFORMED; int _is_occupied = -1; int is_failed = 0; /* Check the legality of initialization process. */ if (g_model_state == MODEL_ON) { return INIT_ERROR_REDUNDANCY; /* 重复初始化 */ } /* Initialization of resources */ g_model_state = MODEL_OFF; _is_occupied = check_resources_occupied(); if (_is_occupied) { free_resources(); } is_failed = alloc_resources(); if (is_failed) { res = INIT_ERROR_MEMORY_INIT_FAILED; }else{ res = INIT_SUCCESS; g_model_state = MODEL_ON; } return res; } SCENE_INIT_STATE scene_recognition_reset(void) { SCENE_INIT_STATE _process_res = INIT_RESET_FAILED; int _is_occupied = 0; int _is_failed = 0; if (g_model_state == MODEL_OFF) { _process_res = INIT_RESET_FAILED; } else { _is_occupied = check_resources_occupied(); if (_is_occupied) { free_resources(); } g_model_state = MODEL_OFF; _is_failed = alloc_resources(); if (_is_failed) { _process_res = INIT_RESET_FAILED; g_model_state = MODEL_OFF; } else { _process_res = INIT_RESET_SUCCESS; g_model_state = MODEL_ON; } } return _process_res; } /**---------------------------------------------------------------------- * Function : sceneRecognitionProc * Description : GNSS场景识别处理流程： * 1）将nmea转换为gnss * 2) 通过accuracy、snr、卫星数量分析GNSS信号质量 * 3) 返回结果 * Input : PDR的nmea结构体 * Return : 场景识别结果 * Author : logzhan * Date : 2020/02/18 * * * * *---------------------------------------------------------------------**/ SCENE_RECOGNITION_RESULT sceneRecognitionProc(const lct_nmea *nmea) { SCENE_RECOGNITION_RESULT res = RECOG_UNKNOWN; int transRes = 0; SIGNAL_QUALITY detRes = SIG_UNKNOWN; GnssInfo gnssInfo; // 将nmea转换为gnss transRes = lctnmea2Gnssinfo(&gnssInfo, nmea); switch (transRes) { case -1: res = RECOG_ERROR_PARAM_INCRECT; break; case 1: res = RECOG_ERROR_NMEA_INFO_MISS; break; case 0: // GNSS信号检测结果 detRes = detGnssSignal(&gnssInfo); if (detRes == GOOD) { res = RECOG_OPEN_AREA; } else if (detRes == BAD) { res = RECOG_MULTIPATH; } else /*UNKOWN but is not exist*/ { res = RECOG_UNKNOWN; } break; default: res = RECOG_UNKNOWN; } return res; } /**---------------------------------------------------------------------- * Function : isOpenArea * Description : GNSS开阔地检测，通过输入GNSS信号，分析当前信号质量，从而确定 * 目标是否处于开阔场地。 * Return : 0 : 非开阔地 1：开阔地(信号质量较好) * Author : Zhang Jingrui, Geek ID: 11082826 * Date : 2020/02/18 logzhan *---------------------------------------------------------------------**/ int isOpenArea(const lct_nmea *nmea) { SCENE_RECOGNITION_RESULT type = sceneRecognitionProc(nmea); // 如果是开阔地则返回1 return (type == RECOG_OPEN_AREA); } SCENE_DESTROY_STATE scene_recognition_destroy(void) { SCENE_DESTROY_STATE res = DESTROY_INVALID; if (g_model_state == MODEL_OFF){ res = DESTROY_INVALID; }else{ // 释放资源 free_resources(); g_model_state = MODEL_OFF; res = DESTROY_SUCCESS; } return res; } /* static function defination -----------------------------------*/ /*===============================================================*/ static int check_resources_occupied(void) { int res = 0; if (g_gnss_info_buffer != NULL){ ++res; } return res; } static int free_resources(void) { int res = 0; if (g_gnss_info_buffer != NULL) { free(g_gnss_info_buffer); } g_gnss_info_buffer = NULL; /* The pointer should be set NULL after free() for memory safety. */ g_gnss_info_buffer_trail = 0; /* 0 because of the memory of g_gnss_info_buffer is NULL now. */ return res; } static int alloc_resources(void) { int _process_res = 0; if (g_gnss_info_buffer != NULL) { /* Check the buffer's occupied. This branch is generally because of the */ /* functions flow call errors. */ _process_res = -1; return _process_res; } GnssInfo *ptr = (GnssInfo *)malloc(sizeof(GnssInfo) * g_gnss_info_buffer_size); if (ptr != NULL) { g_gnss_info_buffer = ptr; g_gnss_info_buffer_trail = 0; _process_res = 0; } else { /* Failed to apply for memory.*/ _process_res = 1; } return _process_res; } static int lctnmea2Gnssinfo(GnssInfo *_dst, const lct_nmea * _src_nmea) { int _parse_res_state = 0; /* Temporarily deprecated. */ /* int _gsa_used_prn[3][_CONSTELLATION_USED_CAPACITY] = {0}; */ /* GPGSA, GLGSA, GAGSA */ int _i_first = 0; int _i_second = 0; int _offset[6] = { _GP_SVID_OFFSET, _GP_INDEX_OFFSET, _GL_SVID_OFFSET, _GL_INDEX_OFFSET, _GA_SVID_OFFSET, _GA_INDEX_OFFSET }; int _index = -1; /* Detecting the NULL pamrameters inputed. */ if (_src_nmea == NULL || _dst == NULL) { _parse_res_state = -1; } /* Detecting the _src_nmea if it wasn't updated. */ else if (_src_nmea->update == 0) { _parse_res_state = 1; } /*****************************************************************/ /* DELETE: abandon this branch for the stability of the program. */ /* Detecting the incorrect accuracy value. */ /*else if ((_src_nmea->accuracy.update == 0) || (_src_nmea->accuracy.update == 0) ) { _parse_res_state = 2; }*/ /****************************************************************/ /* update the the _GnssInfo. */ else { /* initialize the memory space pointed by _dst to all 0. */ gnss_info_st_cpy(_dst, NULL); /* pick the raw accuracy from lct_nmea::loc_accuracy. Therefore _dst->accuracy could be equal with ITEM_INVALID. */ _dst->accuracy = _src_nmea->accuracy.err; /* pick the aviable satellites PRN from GxGSA and match the SNR value in GxGSV where Gx is the presented to GP, GA or GL. */ for (_i_first = 0 ; _i_first < 3 ; ++ _i_first) /* 0 - GP, 1 - GL, 2 - GA */ { /* make sure the GxGSA and GxGSV has been update in the epoch */ if (_src_nmea->gsa[_i_first].update == 0 || _src_nmea->gsv[_i_first].update == 0) { continue; } /* copy all the PRN number in GSA to the _dst pointed _GnssInfo */ for (_i_second = 0 ; _i_second < _CONSTELLATION_USED_CAPACITY && _src_nmea->gsa[_i_first].prn[_i_second] != 0 && _src_nmea->gsa[_i_first].prn[_i_second] != ITEM_INVALID ; ++ _i_second) { _index = _src_nmea->gsa[_i_first].prn[_i_second] - _offset[_i_first*2] + _offset[_i_first*2 + 1]; if (_index > 0) { _dst->sat_used_list[_index] = 1; } } /* copy all the SNR values in GSV to the _dst pointed _GnssInfo */ for (_i_second = 0 ; (_i_second < _CONSTELLATION_VISIBLE_CAPACITY) && _src_nmea->gsv[_i_first].satellites_data[_i_second].snr != ITEM_INVALID ; ++ _i_second) { _index = _src_nmea->gsv[_i_first].satellites_data[_i_second].prn - _offset[_i_first*2] + _offset[_i_first*2 + 1]; if (_index > 0) { _dst->snr_list[_index] = (float)(_src_nmea->gsv[_i_first].satellites_data[_i_second].snr); } } } _dst->update = 1; _parse_res_state = 0; } return _parse_res_state; } /**--------------------------------------------------------------------- * Function : detGnssSignal * Description : 检测GPS信号是否良好，原理： * 1）当Accuracy在0到5之间，那么是较好的信号 * 2）如果Accuracy在5到10之间，那么利用卫星总数和使用数量判断 * GPS质量好坏 * 3）如果Accuracy在10以外，那么说明GPS信号不好 * Date : 2020/02/18 logzhan *---------------------------------------------------------------------**/ static SIGNAL_QUALITY detGnssSignal(const GnssInfo* _src) { SIGNAL_QUALITY procRes = SIG_UNKNOWN; int i = 0; float accuracy = 0; float snr_large_rate = 0; int sat_used_num = 0; // 卫星使用数量 /* Simulating a Queue data structure which can be optimazated in the future. */ /* move all the elements to the right one memory and abandon the rightmost one.*/ for (i = 0 ; i < g_gnss_info_buffer_size - 1 ; ++ i) { gnss_info_st_cpy((g_gnss_info_buffer + i + 1) , (g_gnss_info_buffer + i) ); } /* Enqueue where the new element struct is put in the leftmost gpsFusionLocation of the array. */ gnss_info_st_cpy((g_gnss_info_buffer), _src); if (g_gnss_info_buffer_trail < g_gnss_info_buffer_size) { ++ g_gnss_info_buffer_trail; } /* make sure _src->accuracy is available otherwise the quality of signal is not correct. */ if (fabs(_src->accuracy - ITEM_INVALID) < FLT_EPSILON ) { procRes = SIG_UNKNOWN; return procRes; } /* features extraction */ for (i = 0 ; i < _GNSS_INDEX_LEN ; ++ i) { if (_src->sat_used_list[i] == 1 && _src->snr_list[i] > 20) { ++ snr_large_rate; } } sat_used_num = _src->sat_visible_number; accuracy = _src->accuracy; snr_large_rate = sat_used_num > 0 ? (snr_large_rate / sat_used_num) : 0 ; /* classification process BEGIN */ if (accuracy > 0 && accuracy < (5 + FLT_EPSILON) ) { procRes = GOOD; } else if (accuracy > (10 + FLT_EPSILON) ) { procRes = BAD; } else { /* 5 <= _accuracy <= 10 */ if (snr_large_rate < 0.878676) { if (snr_large_rate < 0.781746) { procRes = BAD; } else { if (sat_used_num < 18.5) { procRes = BAD; } else { procRes = GOOD; } } }else{ if (accuracy > 0 && accuracy < 9.2) { procRes = GOOD; } else { procRes = BAD; } } } /* classification process END */ return procRes; } static void gnss_info_st_cpy(GnssInfo *_dst, const GnssInfo *_src) { if (_dst != NULL && _src == NULL) { /* initialize _dst if _src is a empty pointer. */ _dst->update = 0; _dst->local_timestamp = 0; _dst->accuracy = 0; _dst->sat_visible_number = 0; memset(_dst->snr_list, 0, sizeof(_dst->snr_list) ); memset(_dst->sat_used_list, 0, sizeof(_dst->sat_used_list) ); } else if (_dst != NULL && _src != NULL && _dst != _src) { /* deep copy from _src to _dst if both of them isn't empty and not equal. */ _dst->update = _src->update; _dst->local_timestamp = _src->local_timestamp; _dst->accuracy = _src->accuracy; _dst->sat_visible_number = _src->sat_visible_number; memcpy(_dst->snr_list, _src->snr_list, sizeof(_src->snr_list) ); memcpy(_dst->sat_used_list, _src->sat_used_list, sizeof(_src->sat_used_list) ); } else { /* _dst and _src are both NULL pointer, then PASS. */ /* _dst is NULL pointer, then PASS. */ /* _dst and _src are both pointer to the same memory, then PASS. */ } }