logzhan
/
PDR
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity
PDR/1.Software/PDR 1.01/src/scene_recognition.c

497 lines
18 KiB
C
Raw Normal View History

上传第一版代码
2022-09-15 16:35:25 +08:00
/********************************************************************/
/* 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 <stdio.h>
#include <math.h> /* fabs() */
#include <stdlib.h> /* malloc(), free() */
/* @Lai Zhilong added below code*/
#if !defined(LOCATION_PLATFORM_QCOM_MODEM) /* The Macro is in "custom.h" */
#include <malloc.h>
/* @Zhang Jingrui: The <malloc.h> header is depercated (and quite Linux specific, */
/* on which it defines non-standard functions like mallinfo(3)). Notice that the */
/* header <stdlib.h> is defined by C89 (and later) standards, but not <malloc.h>. */
#endif
#include <string.h> /* memset() */
#include <float.h> /* 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 <sensors.h>. */
/* 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; /* <20>ظ<EFBFBD><D8B8><EFBFBD>ʼ<EFBFBD><CABC> */
}
/* 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<EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʶ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>̣<EFBFBD>
* 1<EFBFBD><EFBFBD><EFBFBD><EFBFBD>nmeaת<EFBFBD><EFBFBD>Ϊgnss
* 2) ͨ<EFBFBD><EFBFBD>accuracy<EFBFBD><EFBFBD>snr<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>GNSS<EFBFBD>ź<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
* 3) <EFBFBD><EFBFBD><EFBFBD>ؽ<EFBFBD><EFBFBD><EFBFBD>
* Input : PDR<EFBFBD><EFBFBD>nmea<EFBFBD>ṹ<EFBFBD><EFBFBD>
* Return : <EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʶ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
* 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;
// <20><>nmeaת<61><D7AA>Ϊ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<53>źż<C5BA><C5BC><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
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<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ؼ<EFBFBD><EFBFBD>⣬ͨ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>GNSS<EFBFBD>źţ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ǰ<EFBFBD>ź<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ӷ<EFBFBD>ȷ<EFBFBD><EFBFBD>
* Ŀ<EFBFBD><EFBFBD><EFBFBD>Ƿ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>ڿ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ء<EFBFBD>
* Return : 0 : <EFBFBD>ǿ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> 1<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>(<EFBFBD>ź<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ϻ<EFBFBD>)
* Author : Zhang Jingrui, Geek ID: 11082826
* Date : 2020/02/18 logzhan
*---------------------------------------------------------------------**/
int isOpenArea(const lct_nmea *nmea)
{
SCENE_RECOGNITION_RESULT type = sceneRecognitionProc(nmea);
// <20><><EFBFBD><EFBFBD><EFBFBD>ǿ<EFBFBD><C7BF><EFBFBD><EFBFBD><EFBFBD><EFBFBD>򷵻<EFBFBD>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{
// <20>ͷ<EFBFBD><CDB7><EFBFBD>Դ
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 : <EFBFBD><EFBFBD><EFBFBD><EFBFBD>GPS<EFBFBD>ź<EFBFBD><EFBFBD>Ƿ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>ã<EFBFBD>ԭ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>
* 1<EFBFBD><EFBFBD><EFBFBD><EFBFBD>Accuracy<EFBFBD><EFBFBD>0<EFBFBD><EFBFBD>5֮<EFBFBD>䣬<EFBFBD><EFBFBD>ô<EFBFBD>ǽϺõ<EFBFBD><EFBFBD>ź<EFBFBD>
* 2<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Accuracy<EFBFBD><EFBFBD>5<EFBFBD><EFBFBD>10֮<EFBFBD>䣬<EFBFBD><EFBFBD>ô<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʹ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ж<EFBFBD>
* GPS<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>û<EFBFBD>
* 3<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Accuracy<EFBFBD><EFBFBD>10<EFBFBD><EFBFBD><EFBFBD>⣬<EFBFBD><EFBFBD>ô˵<EFBFBD><EFBFBD>GPS<EFBFBD>źŲ<EFBFBD><EFBFBD><EFBFBD>
* 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; // <20><><EFBFBD><EFBFBD>ʹ<EFBFBD><CAB9><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
/* 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. */
}
}