重构代码，删除冗余版本
parent
f77aeb5ab8
commit
318dad3572
|
|
@ -1,43 +0,0 @@
|
|||
#ifndef __FILTER_H__
|
||||
#define __FILTER_H__
|
||||
/* Macro Declaration ---------------------------------------------------------------------------- */
|
||||
#define FILTER_COEF_MAX_LENGTH 71
|
||||
#define FILTER_FIR_COEF_MAX_LENGTH 71
|
||||
|
||||
/* Structure Declaration ------------------------------------------------------------------------ */
|
||||
typedef struct FILTER { // iir or fir filter
|
||||
int reset; // reset flag
|
||||
int order; // coefficient array length, not filter order
|
||||
double H0; // dc signal gain
|
||||
double b[FILTER_COEF_MAX_LENGTH]; // b coefficient
|
||||
double a[FILTER_COEF_MAX_LENGTH]; // a coefficient
|
||||
double yout[FILTER_COEF_MAX_LENGTH]; // yout buffer
|
||||
double xin[FILTER_COEF_MAX_LENGTH]; // xin buffer
|
||||
} FILTER;
|
||||
|
||||
typedef struct FILTER_FIR { // fir filter
|
||||
int reset; // reset flag
|
||||
int order; // coefficient array length, not filter order
|
||||
double H0; // dc signal gain
|
||||
double b[FILTER_FIR_COEF_MAX_LENGTH]; // b coefficient
|
||||
double xin[FILTER_FIR_COEF_MAX_LENGTH]; // xin buffer
|
||||
} FILTER_FIR;
|
||||
|
||||
typedef struct DELAYER { // delayer
|
||||
int reset;
|
||||
int delay; // delay point number
|
||||
double xin[FILTER_FIR_COEF_MAX_LENGTH]; // xin buffer
|
||||
} DELAYER;
|
||||
|
||||
/* Function Declaration ------------------------------------------------------------------------- */
|
||||
extern void FILTER_reset(FILTER *f);
|
||||
extern void FILTER_FIR_reset(FILTER_FIR *f);
|
||||
extern void FILTER_setCoef(FILTER *f, int order, double *b, double *a, double H0);
|
||||
extern void FILTER_FIR_setCoef(FILTER_FIR *f, int order, double *b, double H0);
|
||||
extern double FILTER_filter(FILTER *f, double x);
|
||||
extern double FILTER_FIR_filter(FILTER_FIR *f, double x);
|
||||
extern void DELAY_reset(DELAYER *d);
|
||||
extern void DELAY_setCoef(DELAYER *d, int delay);
|
||||
extern double DELAY_delay(DELAYER *d, double x);
|
||||
|
||||
#endif
|
||||
|
|
@ -1,43 +0,0 @@
|
|||
#pragma once
|
||||
#ifndef _LOCATION_TOOL_H_
|
||||
#define _LOCATION_TOOL_H_
|
||||
|
||||
#include <stddef.h>
|
||||
|
||||
#ifndef PI
|
||||
//#define PI 3.14159265358979323846
|
||||
#endif // !PI
|
||||
|
||||
#ifndef EARTH_RADIUS
|
||||
#define EARTH_RADIUS 6371008.8
|
||||
#endif
|
||||
|
||||
typedef struct DoublePair {
|
||||
double x;
|
||||
double y;
|
||||
} DoublePair;
|
||||
typedef DoublePair PlaneCoordinate;
|
||||
|
||||
typedef struct LatLng{
|
||||
double lat;
|
||||
double lon;
|
||||
} LatLng;
|
||||
|
||||
typedef struct DoublePairList{
|
||||
double *x;
|
||||
double *y;
|
||||
size_t length;
|
||||
} DoublePairList;
|
||||
typedef struct LocationList{
|
||||
double *lat;
|
||||
double *lon;
|
||||
size_t length;
|
||||
} LocationList;
|
||||
|
||||
LatLng ProjPointOfLatLng(LatLng point, LatLng linePointA, LatLng linePointB);
|
||||
DoublePair ProjPoint(DoublePair point, DoublePair linePointA, DoublePair linePointB);
|
||||
|
||||
//PlaneCoordinate WGS84_XYZ(const LatLng lla);
|
||||
double CalculateDistance(LatLng pointA, LatLng pointB);
|
||||
|
||||
#endif // !_LOCATION_TOOL_H_
|
||||
|
|
@ -1,77 +0,0 @@
|
|||
#ifndef _PDR_BUFFER_H_
|
||||
#define _PDR_BUFFER_H_
|
||||
|
||||
#ifdef __cplusplus
|
||||
extern "C" {
|
||||
#endif
|
||||
|
||||
/* Header File Including ------------------------------------------------------------------------ */
|
||||
|
||||
/* Macro Declaration ---------------------------------------------------------------------------- */
|
||||
#define BUFFER_LONG_LENGTH 256
|
||||
#define BUFFER_SHORT_LENGTH 10
|
||||
#define BUFFER_TYPE_STACK 0
|
||||
#define BUFFER_TYPE_QUEUE 1
|
||||
#define BUFFER_NO_ERROR 0
|
||||
#define BUFFER_WRONG_PARAMETER 1
|
||||
#define BUFFER_OUT_OF_MEMORY 2
|
||||
|
||||
/* Structure Declaration ------------------------------------------------------------------------ */
|
||||
#pragma pack (4)
|
||||
typedef struct BUFFER {
|
||||
char name[20];
|
||||
int type;
|
||||
int length;
|
||||
int _bottom;
|
||||
int _top;
|
||||
int reserved;
|
||||
double sum;
|
||||
double mean;
|
||||
float data[BUFFER_LONG_LENGTH + 1];
|
||||
} BUFFER;
|
||||
|
||||
typedef struct BUFFER_LONG {
|
||||
char name[20];
|
||||
int type;
|
||||
int length;
|
||||
int _bottom;
|
||||
int _top;
|
||||
int reserved;
|
||||
double sum;
|
||||
double mean;
|
||||
float data[BUFFER_LONG_LENGTH + 1];
|
||||
} BUFFER_LONG;
|
||||
|
||||
typedef struct BUFFER_SHORT {
|
||||
char name[20];
|
||||
int type;
|
||||
int length;
|
||||
int _bottom;
|
||||
int _top;
|
||||
int reserved;
|
||||
double sum;
|
||||
double mean;
|
||||
float data[BUFFER_SHORT_LENGTH + 1];
|
||||
} BUFFER_SHORT;
|
||||
#pragma pack ()
|
||||
|
||||
/* Global Variable Declaration ------------------------------------------------------------------ */
|
||||
|
||||
/* Function Declaration ------------------------------------------------------------------------- */
|
||||
int Buffer_initialize(BUFFER *buffer, const char *name, int type, int length);
|
||||
int Buffer_clear(BUFFER *buffer);
|
||||
int Buffer_count(BUFFER *buffer, int *count);
|
||||
int Buffer_top(BUFFER *buffer, float *value);
|
||||
int Buffer_bottom(BUFFER *buffer, float *value);
|
||||
int Buffer_pop(BUFFER *buffer, float *value);
|
||||
int buffer_push(BUFFER *buffer, float value);
|
||||
int Buffer_get(BUFFER *buffer, float *value, int index);
|
||||
int Buffer_mean(BUFFER *buffer, float *mean);
|
||||
int Buffer_var(BUFFER *buffer, float *var);
|
||||
int Buffer_std(BUFFER *buffer, float *std);
|
||||
|
||||
#ifdef __cplusplus
|
||||
}
|
||||
#endif
|
||||
|
||||
#endif
|
||||
|
|
@ -1,110 +0,0 @@
|
|||
#ifdef __cplusplus
|
||||
extern "C" {
|
||||
#endif
|
||||
#ifndef _PARSEDATA_H_
|
||||
#define _PARSEDATA_H_
|
||||
|
||||
#include "pdr_sensor.h"
|
||||
|
||||
#define NAEA_LAST_TIME 500
|
||||
#define IMU_LAST_COUNT 10
|
||||
|
||||
#define FLOAT_TO_INT 1000000
|
||||
|
||||
/**************************************************************************
|
||||
* Description : NMEA数据和传感器数据解析
|
||||
* : NMEA数据包括RMC,GGA,GSA,GSV,以及安卓定位信息语句
|
||||
传感器数据包括加速度计，陀螺仪，磁力计
|
||||
**************************************************************************/
|
||||
|
||||
char * strtok_ct(char * s, const char * delim);
|
||||
|
||||
/**************************************************************************
|
||||
* Description : 解析惯性传感器数据
|
||||
* Input : pt， 传感器数据字符串
|
||||
ts， 手机开机时间戳，ms
|
||||
sstp，安卓中传感器类型标识符
|
||||
* Output : imu_p， 惯性传感器结构体
|
||||
**************************************************************************/
|
||||
void ParseIMU(char* pt, imu *imu_p, double ts, int sstp);
|
||||
|
||||
/**************************************************************************
|
||||
* Description : 解析NMEA数据
|
||||
* Input : pt， NMEA数据字符串
|
||||
ts， 手机开机时间戳，ms
|
||||
* Output : ln， NMEA数据结构体
|
||||
**************************************************************************/
|
||||
void parseNMEA(char* pt, lct_nmea *ln, double ts);
|
||||
|
||||
/**************************************************************************
|
||||
* Description : 解析GGA协议
|
||||
* Input : pt， NMEA数据字符串
|
||||
ts， 手机开机时间戳，ms
|
||||
* Output : ln， NMEA数据结构体
|
||||
**************************************************************************/
|
||||
void ParseGGA(char* pt, lct_nmea *ln, double ts);
|
||||
|
||||
/**************************************************************************
|
||||
* Description : 解析RMC协议
|
||||
* Input : pt， NMEA数据字符串
|
||||
ts， 手机开机时间戳，ms
|
||||
* Output : ln， NMEA数据结构体
|
||||
**************************************************************************/
|
||||
void ParseRMC(char* pt, lct_nmea *ln, double ts);
|
||||
|
||||
/**************************************************************************
|
||||
* Description : 解析GSV协议
|
||||
* Input : pt， NMEA数据字符串
|
||||
ts， 手机开机时间戳，ms
|
||||
* Output : ln， NMEA数据结构体
|
||||
**************************************************************************/
|
||||
void parseGSV(char* pt, lct_nmea *ln, double ts);
|
||||
|
||||
/**************************************************************************
|
||||
* Description : 解析GSA协议
|
||||
* Input : pt， NMEA数据字符串
|
||||
ts， 手机开机时间戳，ms
|
||||
* Output : ln， NMEA数据结构体
|
||||
**************************************************************************/
|
||||
void ParseGSA(char* pt, lct_nmea *ln, double ts);
|
||||
|
||||
/**----------------------------------------------------------------------
|
||||
* Function : parseLocAccuracy
|
||||
* Description : 解析GPS的Accuracy精度参数
|
||||
* Date : 2020/7/9 yuanlin@vivo.com &logzhan
|
||||
*---------------------------------------------------------------------**/
|
||||
void parseLocAccuracy(char* pt, lct_nmea *ln, double ts);
|
||||
|
||||
/**************************************************************************
|
||||
* Description : 检测NMEA数据是否解析完整
|
||||
* Input : ln， NMEA数据结构体
|
||||
**************************************************************************/
|
||||
void preprocessNMEA(lct_nmea *ln);
|
||||
|
||||
int pdr_parseLineStr(char* line, imu* imu_p, lct_nmea* ln);
|
||||
|
||||
/**----------------------------------------------------------------------
|
||||
* Function : HexToDec
|
||||
* Description : 十六进制数转换为十进制数
|
||||
* Date : 2022-09-15 logzhan
|
||||
*---------------------------------------------------------------------**/
|
||||
long HexToDec(char *source);
|
||||
|
||||
/**----------------------------------------------------------------------
|
||||
* Function : pdr_int2Hex
|
||||
* Description : 十六进制数转换为十进制数
|
||||
* Date : 2020/7/9 yuanlin@vivo.com & logzhan
|
||||
*---------------------------------------------------------------------**/
|
||||
char * Int2Hex(int a, char *buffer);
|
||||
|
||||
|
||||
/**----------------------------------------------------------------------
|
||||
* Function : pdr_getIndexOfSigns
|
||||
* Description : 返回ch字符在sign数组中的序号
|
||||
* Date : 2020/7/9 yuanlin@vivo.com &logzhan
|
||||
*---------------------------------------------------------------------**/
|
||||
int pdr_getIndexOfSigns(char ch);
|
||||
#endif
|
||||
#ifdef __cplusplus
|
||||
}
|
||||
#endif
|
||||
|
|
@ -1,48 +0,0 @@
|
|||
#ifndef _PDR_AHRS_H_
|
||||
#define _PDR_AHRS_H_
|
||||
|
||||
#ifdef __cplusplus
|
||||
extern "C" {
|
||||
#endif
|
||||
|
||||
/* Header File Including ------------------------------------------------------------------------ */
|
||||
|
||||
#include "pdr_sensor.h"
|
||||
|
||||
/* Macro Declaration ---------------------------------------------------------------------------- */
|
||||
#define AHRS_SAMPLE_FREQ 100 // ARHS算法的传感器采样频率
|
||||
#define IMU_SAMPLING_FREQUENCY 100
|
||||
#define AHRS_KP 0.500
|
||||
#define AHRS_TYPE_GYRO 4
|
||||
#define AHRS_TYPE_ACCE 1
|
||||
#define AHRS_TYPE_MAGN 2
|
||||
#define AHRS_STATUS_RESET_PHASE_0 0x80
|
||||
#define AHRS_STATUS_RESET_PHASE_1 0x40
|
||||
#define AHRS_STATUS_RESET (AHRS_STATUS_RESET_PHASE_0|AHRS_STATUS_RESET_PHASE_1)
|
||||
#define AHRS_STATUS_STABLE 0x20
|
||||
|
||||
/* Struct Declaration --------------------------------------------------------------------------- */
|
||||
|
||||
|
||||
/* Function Declaration ------------------------------------------------------------------------- */
|
||||
|
||||
void pdr_initAhrs(void);
|
||||
void pdr_ahrsReset(void);
|
||||
|
||||
/**---------------------------------------------------------------------
|
||||
* Function : fv3Norm
|
||||
* Description : 三维浮点数向量归一化
|
||||
* Date : 2021/01/26 yuanlin@vivocom &&
|
||||
*
|
||||
*
|
||||
*---------------------------------------------------------------------**/
|
||||
void fv3Norm(float* vx, float* vy, float* vz);
|
||||
|
||||
int pdr_imuUpdateAhrs(imu* imu);
|
||||
|
||||
|
||||
#ifdef __cplusplus
|
||||
}
|
||||
#endif
|
||||
|
||||
#endif // for __AHRS_H
|
||||
|
|
@ -1,157 +0,0 @@
|
|||
/******************** (C) COPYRIGHT 2020 Geek************************************
|
||||
* File Name : pdr_api.h
|
||||
* Department : Sensor Algorithm Team
|
||||
* Current Version : V2.0(compare QCOM SAP 5.0)
|
||||
* Author : logzhan
|
||||
* Date of Issued : 2021.01.26
|
||||
* Comments : PDR 对外部接口
|
||||
********************************************************************************/
|
||||
#ifndef _PDR_API_H_
|
||||
#define _PDR_API_H_
|
||||
|
||||
#ifdef __cplusplus
|
||||
extern "C" {
|
||||
#endif
|
||||
|
||||
/* Header File Including -----------------------------------------------------*/
|
||||
#include <stdio.h>
|
||||
#include <math.h>
|
||||
#include <string.h>
|
||||
#include "pdr_sensor.h"
|
||||
#include "pdr_base.h"
|
||||
#include "pdr_util.h"
|
||||
|
||||
/* Macro Definition ----------------------------------------------------------*/
|
||||
#define IMU_LAST_COUNT 10
|
||||
|
||||
typedef struct _PosFusion{
|
||||
double lat;
|
||||
double lon;
|
||||
double gpsLat;
|
||||
double gpsLon;
|
||||
double t;
|
||||
int vaild;
|
||||
}PosFusion;
|
||||
|
||||
/* Function Declaration ------------------------------------------------------*/
|
||||
|
||||
/**---------------------------------------------------------------------
|
||||
* Function : pdr_algorithmInit
|
||||
* Description : PDR算法主初始化流程，包括PDR导航初始化、计步器状态初始化以及轨
|
||||
* 迹平滑窗口的初始化
|
||||
* Date : 2020/7/3 logzhan
|
||||
*---------------------------------------------------------------------**/
|
||||
void Algorithm_Init(void);
|
||||
|
||||
/**---------------------------------------------------------------------
|
||||
* Function : pdr_locationMainLoop
|
||||
* Description : PDR定位主循环，主要处理来自上层输入的传感器数据并解算位置
|
||||
* 迹平滑窗口的初始化
|
||||
* Input : ss_data， 传感器数据
|
||||
nmea_data, NMEA数据
|
||||
result，定位结果
|
||||
* Output : int, 输出标志位
|
||||
* Date : 2020/7/3 logzhan
|
||||
*---------------------------------------------------------------------**/
|
||||
int pdr_locationMainLoop(imu *ss_data, lct_nmea *nmea_data, lct_fs *result,
|
||||
FILE *fp_gps);
|
||||
|
||||
/**---------------------------------------------------------------------
|
||||
* Function : pdr_locationMainLoopStr
|
||||
* Description : PDR定位主循环，主要处理来自上层输入的传感器数据并解算位置
|
||||
* 迹平滑窗口的初始化
|
||||
* Date : 2020/07/03 logzhan
|
||||
* 2020/02/02 logzhan : 增加宏控制是否开启输出平滑
|
||||
*---------------------------------------------------------------------**/
|
||||
int pdr_locationMainLoopStr(char* line, lct_fs* result);
|
||||
|
||||
/**---------------------------------------------------------------------
|
||||
* Function : pdr_initRmc
|
||||
* Description : 初始化RMC结构体
|
||||
* Input : rmc， RMC结构体
|
||||
* Date : 2020/7/3 logzhan
|
||||
*---------------------------------------------------------------------**/
|
||||
void RMC_Init(lct_nmea_rmc * rmc);
|
||||
|
||||
/**---------------------------------------------------------------------
|
||||
* Function : pdr_initNmeaFlg
|
||||
* Description : 初始化GPS的NMEA协议的相关标志位
|
||||
* Input : ln， NMEA结构体
|
||||
* Date : 2020/7/3 logzhan
|
||||
*---------------------------------------------------------------------**/
|
||||
void NmeaFlag_Init(lct_nmea * ln);
|
||||
|
||||
/**---------------------------------------------------------------------
|
||||
* Function : clearNmeaFlg
|
||||
* Description : 清空Nmea标志位,功能和init相同
|
||||
* Date : 2020/7/4 logzhan
|
||||
*---------------------------------------------------------------------**/
|
||||
void clearNmeaFlg(lct_nmea * ln);
|
||||
|
||||
/**---------------------------------------------------------------------
|
||||
* Function : pdr_saveGnssInfo
|
||||
* Description : 保存GPS相关信息
|
||||
* Date : 2020/7/3 logzhan
|
||||
*---------------------------------------------------------------------**/
|
||||
void pdr_saveGnssInfo(lct_nmea* nmea_data, lct_fs* result, FILE* fp_gps);
|
||||
|
||||
/**----------------------------------------------------------------------
|
||||
* Function : pdr_libVersion
|
||||
* Description : pdr库版本号
|
||||
* Date : 2020/8/3 logzhan
|
||||
*---------------------------------------------------------------------**/
|
||||
const char* pdr_libVersion(void);
|
||||
|
||||
/**----------------------------------------------------------------------
|
||||
* Function : pdr_closeAlgorithm
|
||||
* Description : 关闭PDR算法,释放资源
|
||||
* Date : 2020/8/3 logzhan
|
||||
*---------------------------------------------------------------------**/
|
||||
void pdr_closeAlgorithm();
|
||||
|
||||
|
||||
|
||||
#ifdef _WIN32 // 主要用于支持win32调试使用，用于dll接口导出
|
||||
|
||||
/**----------------------------------------------------------------------
|
||||
* Function : LibPDR_Init
|
||||
* Description : 初始化PDR算法(DLL调用)
|
||||
* Date : 2020/8/3 logzhan
|
||||
*---------------------------------------------------------------------**/
|
||||
__declspec(dllexport) void LibPDR_Init();
|
||||
|
||||
/**----------------------------------------------------------------------
|
||||
* Function : pdrSumulatorUpdateGpsStep
|
||||
* Description : 仿真器按照GPS单步执行(DLL调用)
|
||||
* Date : 2020/8/3 logzhan
|
||||
*---------------------------------------------------------------------**/
|
||||
__declspec(dllexport) PosFusion pdrSumulatorUpdateGpsStep(int useGpsFlg);
|
||||
|
||||
/**----------------------------------------------------------------------
|
||||
* Function : setSimulatorFileCsvPath
|
||||
* Description : 设置仿真文件的路径(DLL调用)
|
||||
* Date : 2020/8/3 logzhan
|
||||
*---------------------------------------------------------------------**/
|
||||
__declspec(dllexport) void setSimulatorFileCsvPath(char* path);
|
||||
|
||||
/**----------------------------------------------------------------------
|
||||
* Function : closePdrAlgo
|
||||
* Description : 关闭pdr算法，释放相关内存，用于dll调用(DLL调用)
|
||||
* Date : 2020/8/3 logzhan
|
||||
*---------------------------------------------------------------------**/
|
||||
__declspec(dllexport) void closePdrAlgo();
|
||||
|
||||
/**----------------------------------------------------------------------
|
||||
* Function : setRefGpsYaw
|
||||
* Description : 设置GPS参考角度
|
||||
* Date : 2020/8/3 logzhan
|
||||
*---------------------------------------------------------------------**/
|
||||
__declspec(dllexport) void setRefGpsYaw();
|
||||
|
||||
#endif
|
||||
|
||||
#ifdef __cplusplus
|
||||
}
|
||||
#endif
|
||||
|
||||
#endif
|
||||
|
|
@ -1,345 +0,0 @@
|
|||
/******************** (C) COPYRIGHT 2021 Geek************************************
|
||||
* File Name : pdr_base.h
|
||||
* Department : Sensor Algorithm Team
|
||||
* Current Version : V2.0
|
||||
* Author : yuanlin@vivocom &
|
||||
*
|
||||
*
|
||||
* Date of Issued : 2021.02.01
|
||||
* Comments : PDR 算法基本定义头文件
|
||||
********************************************************************************/
|
||||
#ifndef _PDR_BASE_H_
|
||||
#define _PDR_BASE_H_
|
||||
/* Header File Including -----------------------------------------------------*/
|
||||
#include "stdint.h"
|
||||
#include "pdr_sensor.h"
|
||||
|
||||
/* PDR SYS CFG ---------------------------------------------------------------*/
|
||||
#define PDR_OUTPUT_SMOOTH 0 // 是否开启轨迹平滑
|
||||
|
||||
/* Macro Definition ----------------------------------------------------------*/
|
||||
#define PCA_NUM 200
|
||||
#define ACCURACY_ERR_MAX 1000 // GPS的accuracy最大值,一般用于初始化用
|
||||
#define N 4 // 矩阵维数
|
||||
#define OPEN_SKY 1
|
||||
#define MAX_NO_GPS_PREDICT 10 // 无GPS信息状态，最大位置推算数量
|
||||
#define UN_UPDATE 0 // 非更新状态
|
||||
#define DATA_UPDATE 1 // 数据更新
|
||||
#define ON 1
|
||||
#define OFF 0
|
||||
#define PDR_TRUE 1
|
||||
#define PDR_FALSE 0
|
||||
|
||||
#define TYPE_FIX_NONE 0 // PDR修正结果为不输出
|
||||
#define TYPE_FIX_GPS 2 // PDR修正结果为输出原始GPS
|
||||
#define TYPE_FIX_PDR 1 // PDR修正结果为PDR融合
|
||||
|
||||
// PDR携带方式
|
||||
#define UNKNOWN 0
|
||||
#define FORWARD_UP_RIGHT 1 // 臂包：右臂，头朝上，屏朝外；or左臂，头朝上，屏朝内
|
||||
#define BACKWARD_UP_LEFT 2 // 臂包：左臂，头朝上，屏朝外；or右臂，头朝上，屏朝内
|
||||
#define BACKWARD_DOWN_RIGHT 3 // 臂包：右臂，头朝下，屏朝外；or左臂，头朝下，屏朝内
|
||||
#define FORWARD_DOWN_LEFT 4 // 臂包：左臂，头朝下，屏朝外；or右臂，头朝下，屏朝内
|
||||
#define LEFT_UP_FORWARD 5 // 裤兜：头朝上，屏朝前
|
||||
#define RIGHT_UP_BACKWARD 6 // 裤兜：头朝上，屏朝后
|
||||
#define RIGHT_DOWN_FORWARD 7 // 裤兜：头朝下，屏朝前
|
||||
#define LEFT_DOWN_BACKWARD 8 // 裤兜：头朝下，屏朝后
|
||||
|
||||
|
||||
// PDR相关状态
|
||||
#define PDR_NO_ERROR 0
|
||||
#define PDR_OUT_OF_MEMORY 1
|
||||
#define PDR_MOTION_TYPE_STATIC 0
|
||||
#define PDR_MOTION_TYPE_IRREGULAR 1
|
||||
#define PDR_MOTION_TYPE_HANDHELD 2
|
||||
#define PDR_MOTION_TYPE_SWINGING 3
|
||||
#define PDR_STATUS_RESET 0x80
|
||||
#define PDR_STATUS_CACULATING_AXIS_CEOFFICIENT 0x40
|
||||
#define PDR_STATUS_PCA_STABLE 0x20
|
||||
#define PDR_STATUS_BIAS_STABLE 0x10
|
||||
#define PDR_STATUS_STABLE (PDR_STATUS_PCA_STABLE | PDR_STATUS_BIAS_STABLE)
|
||||
|
||||
// 地球相关常数
|
||||
#define WGS84_RE 6378137 // WGS-84椭球体长半轴
|
||||
#define WGS84_ECCENTR2 6.69437999014e-3 // 第一偏向率的平方，e^2(WGS-84)
|
||||
#define WGS84_OMEGA_E_DOT 7.2921151467e-5 // 地球自转角速度 rad/sec
|
||||
#define WGS84_GRAVITY 9.7803267714 // 地球平均重力加速度 m/s^2
|
||||
|
||||
#define DEG_PER_RADIAN 57.295779513082323 // degrees per radian
|
||||
#define RADIAN_PER_DEG 0.017453292519943 // radians per degree
|
||||
|
||||
#define GPS_SPEED_UNIT 0.5144444 // GPS原始速度转m/s的缩放系数
|
||||
#define PI 3.1415926
|
||||
#define DOUBLE_ZERO 1e-10
|
||||
#define TWO_PI (2.0*PI)
|
||||
#define R2D(x) (x*57.2957795130823)
|
||||
#define D2R(x) (x*3.14159265/180.0)
|
||||
#define USE_BUG_FOR_LOCAL_PARA 1
|
||||
#define BUF_DMT_1 1
|
||||
#define BUF_DMT_2 2
|
||||
|
||||
#define PRINT_ALGO_RUN_TIME 0
|
||||
#define PRINT_CLCT 1
|
||||
#define PATTERN_RECOGNITION_LEN 256 // 缓存256 / SAMPLE_RATE的数量
|
||||
|
||||
#define OPEN_AREA 1 // 开阔区域(GPS信号较好)
|
||||
#define UN_OPEN_AREA 0 // 非开阔区域(GPS信号较弱)
|
||||
|
||||
|
||||
#define DETECTOR_RUN_FREQ 1280 // 用户运动类型检测器检测周期ms
|
||||
#define DETECTOR_TYPE_STATIC 0 // 用户静止
|
||||
#define DETECTOR_TYPE_IRREGULAR 1 // 无规律运动
|
||||
#define DETECTOR_TYPE_HANDHELD 2 // 手持运动
|
||||
#define DETECTOR_TYPE_SWINGING 3 // 摆手运动
|
||||
|
||||
#define ulong unsigned long
|
||||
#define uchar unsigned char
|
||||
|
||||
typedef enum {
|
||||
IS_INITIAL = 0,
|
||||
IS_NORMAL = 1,
|
||||
}pdrStatus;
|
||||
|
||||
|
||||
typedef struct {
|
||||
double xk[N]; // 状态变量Xk xk[0]: 北向x xk[1]：东向y xk[2]：步长 xk[3] ：航向角
|
||||
double p_xk[N]; // 最佳预测变量Xk_predict xk[0]: 北向x xk[1]：东向y xk[2]：步长 xk[3] ：航向角
|
||||
double zk[N];
|
||||
double p_pk[N][N];
|
||||
double pk[N][N];
|
||||
double phi[N][N];
|
||||
double hk[N][N];
|
||||
double q[N][N]; // 卡尔曼滤波的Q矩阵(过程噪声)
|
||||
double r[N][N]; // 卡尔曼滤波R矩阵(观测噪声)
|
||||
double Kk[N][N];
|
||||
double lambda;
|
||||
double plat;
|
||||
double plon;
|
||||
double initHeading;
|
||||
}KfPara;
|
||||
|
||||
typedef struct{
|
||||
int fnum;
|
||||
int deltaStep;
|
||||
float fsum;
|
||||
float meanTime;
|
||||
double lastTime;
|
||||
}StepPredict;
|
||||
|
||||
typedef struct PDR {
|
||||
uint32_t status; // PDR当前状态
|
||||
uint32_t motionType; // 用户运动类型
|
||||
uint32_t noGpsCount; // 没有GPS次数
|
||||
pdrStatus sysStatus; // PDR系统状态
|
||||
int sceneType; // 场景类型：1：开阔区域，0：非开阔区域(信号较弱)
|
||||
int fusionPdrFlg; // 融合PDR位置标志位, 当flg为0时纯输出GPS
|
||||
double pllaInit[3]; // 初始plla坐标
|
||||
double ts; // 时间戳
|
||||
double x0; // 初始北向坐标
|
||||
double y0; // 初始东向坐标
|
||||
// 航向角相关
|
||||
double heading; // PDR方向角
|
||||
double lastHeading; // 上一次PDR方向角
|
||||
double deltaHeading; // 偏航角变化量
|
||||
double insHeadingOffset; // 惯导方向角偏移
|
||||
double imuDeltaHeading; // 没间隔一次GPS信号，PDR变化的角度，用于区分转弯
|
||||
double gpsHeading; // GPS航向角
|
||||
double lastGpsHeading; // 上一次GPS航向角
|
||||
double trackHeading; // GPS轨迹航向角
|
||||
// 速度相关
|
||||
double gpsSpeed; // GPS速度
|
||||
// 步数相关
|
||||
ulong steps; // 当前步数信息
|
||||
ulong lastSteps; // 上一次的步数
|
||||
ulong lastGpsSteps; // 上一次GPS步数
|
||||
ulong deltaStepsPerGps; // 两次GPS更新之间，步数的变化量
|
||||
float motionFreq; // 运动频率
|
||||
double gyroTime; // 陀螺仪时间
|
||||
float axis_ceofficient[3];
|
||||
float axis_direction[2];
|
||||
float pca_direction[2];
|
||||
float pca_accuracy;
|
||||
float bias_direction[2];
|
||||
float bias_accuracy;
|
||||
float cal_direction[2];
|
||||
} PDR;
|
||||
|
||||
typedef struct {
|
||||
int type;
|
||||
float accBuffer[PATTERN_RECOGNITION_LEN];
|
||||
}classifer;
|
||||
|
||||
// 用户运动类型分类器
|
||||
typedef struct DETECTOR {
|
||||
uint32_t type; // 用户运动类别 ： 0:静止运动 1：无规律运动 2：手持运动 3：摆手运动
|
||||
uint32_t lastType;
|
||||
uint64_t tick; // 次数统计，用于调整检测器工作频率
|
||||
} DETECTOR;
|
||||
|
||||
|
||||
DETECTOR *pdr_getDetector(void);
|
||||
|
||||
|
||||
/* Function Declaration ------------------------------------------------------*/
|
||||
|
||||
PDR* pdr_initBase(void);
|
||||
|
||||
/**----------------------------------------------------------------------
|
||||
* Function : pdr_resetData
|
||||
* Description : 重置PDR变量
|
||||
* Date : 2020/7/21
|
||||
*
|
||||
*---------------------------------------------------------------------**/
|
||||
void pdr_resetData(void);
|
||||
|
||||
/**----------------------------------------------------------------------
|
||||
* Function : pdr_computePCA
|
||||
* Description : 进入RESET_PHASE_1\STABLE阶段之后进行PCA计算
|
||||
* Date : 2020/7/21 logzhan
|
||||
*---------------------------------------------------------------------**/
|
||||
void pdr_computePCA(AHRS* ahrs);
|
||||
|
||||
/**---------------------------------------------------------------------
|
||||
* Function : detectorUpdate
|
||||
* Description : 根据新确定的手机携带方式更新g_pdr.status等
|
||||
* Date : 2020/7/20 logzhan
|
||||
*---------------------------------------------------------------------**/
|
||||
void detectorUpdate(DETECTOR* detector);
|
||||
|
||||
|
||||
void gpsUpdateCb(gnss* gps);
|
||||
|
||||
|
||||
/**************************************************************************
|
||||
* Description : 计算每步所耗时间
|
||||
* Input : stepPredict， 步数预测结构体
|
||||
timestamp，时间戳
|
||||
steps_last，上一次步数
|
||||
steps，本次步数
|
||||
**************************************************************************/
|
||||
void calStepLastTime(StepPredict *stepPredict, double timestamp, unsigned long steps_last, unsigned long steps);
|
||||
|
||||
/**************************************************************************
|
||||
* Description : 预测步数
|
||||
* Input : stepPredict， 步数预测结构体
|
||||
timestamp，时间戳
|
||||
steps_last，上一次步数
|
||||
gnssVel，gps速度
|
||||
* Output : int， 步数
|
||||
**************************************************************************/
|
||||
int predictStep(StepPredict *stepPredict, double timestamp, unsigned long steps_last, float gnssVel);
|
||||
|
||||
/**----------------------------------------------------------------------
|
||||
* Function : stateRecognition
|
||||
* Description : 根据加速度及判断当前状态是否平稳 1: 不平稳 0: 平稳
|
||||
* Date : 2020/7/22 logzhan
|
||||
*---------------------------------------------------------------------**/
|
||||
void stateRecognition(float *acc, classifer *sys);
|
||||
|
||||
/**************************************************************************
|
||||
* Description : 根据GPS角度、GPS轨迹角、PDR角度，计算用户行走方向
|
||||
* Input : g_pdr，PDR结构体
|
||||
sys，运动幅度数据结构体
|
||||
gps_yaw，GPS轨迹角
|
||||
G_yaw，GPS方向角
|
||||
ss_data， 传感器数据结构体
|
||||
* Output : double，用户行走方向
|
||||
**************************************************************************/
|
||||
double calPredAngle(PDR *g_pdr, classifer *sys, double gps_yaw, double G_yaw, imu *ss_data);
|
||||
|
||||
/**************************************************************************
|
||||
* Description : 根据GPS角度、GPS轨迹角、PDR角度，计算用户行走方向
|
||||
* Input : g_pdr，PDR结构体
|
||||
steps，本次步数
|
||||
steps_last，上一次步数
|
||||
stepPredict，步数预测结构体
|
||||
pdr_angle，用户行走方向
|
||||
scene_type，GPS用户所处场景（开阔和非开阔）
|
||||
ss_data， 传感器数据结构体
|
||||
pgnss，GPS数据结构体
|
||||
kf，EKF结构体
|
||||
pdr, PDR预测位置成功标志位
|
||||
**************************************************************************/
|
||||
void pdr_insPredict(PDR *g_pdr, StepPredict *stepPredict,imu *ss_data,
|
||||
gnss *pgnss, KfPara *kf);
|
||||
|
||||
/**----------------------------------------------------------------------
|
||||
* Function : pdr_detStatic
|
||||
* Description : 检测用户是否处于静止状态
|
||||
* Date : 2021/01/28 logzhan
|
||||
*---------------------------------------------------------------------**/
|
||||
int pdr_detStatic(PDR *g_pdr, gnss *pgnss, unsigned long delSteps);
|
||||
|
||||
|
||||
/**----------------------------------------------------------------------
|
||||
* Function : pdr_outputGpsPos
|
||||
* Description : 输出GPS位置
|
||||
* Date : 2020/07/08 logzhan
|
||||
*---------------------------------------------------------------------**/
|
||||
void pdr_outputGpsPos(gnss *pgnss, lct_fs *result);
|
||||
|
||||
/**----------------------------------------------------------------------
|
||||
* Function : resetOutputResult
|
||||
* Description : 1、初始化解算初始位置、卡尔曼滤波初始参数
|
||||
* 2、GPS值赋值给result
|
||||
* pgnss，GPS数据结构体
|
||||
* plla_init，待重置NED坐标系原点经纬度
|
||||
* x_init，y_init待重置NED坐标系原点的n和e坐标
|
||||
* kf， EKF数据结构体
|
||||
* Date : 2020/07/08 logzhan
|
||||
*---------------------------------------------------------------------**/
|
||||
int resetOutputResult(gnss *pgnss, PDR* g_pdr, KfPara *kf, lct_fs *result);
|
||||
|
||||
/**----------------------------------------------------------------------
|
||||
* Function : detIsCarMode
|
||||
* Description : 识别是否是车载模式
|
||||
* Input : pgnss，GPS数据结构体
|
||||
g_pdr，PDR结构体
|
||||
delSteps， 与上次相比步数的变化量
|
||||
time，计数器
|
||||
* Output : int，车载模式标志位
|
||||
* Date : 2021/01/28 logzhan
|
||||
*---------------------------------------------------------------------**/
|
||||
int detIsCarMode(gnss *pgnss, PDR *g_pdr, unsigned long delSteps, int *time);
|
||||
|
||||
/**----------------------------------------------------------------------
|
||||
* Function : detPdrToReset
|
||||
* Description : 检测PDR系统是否需要重置，考虑到后续PDR推算失误，或者其他情况
|
||||
* 重置PDR到GPS的位置。
|
||||
* Date : 2021/01/28 logzhan
|
||||
*---------------------------------------------------------------------**/
|
||||
int detPdrToReset(double pdr_angle, int *gpscnt, unsigned long deltsteps, PDR *g_pdr);
|
||||
|
||||
/**************************************************************************
|
||||
* Description : 初始化EKF滤波器
|
||||
* Input : pgnss，GPS数据结构体
|
||||
plla_init，NED坐标系原点经纬度
|
||||
kf， EKF数据结构体
|
||||
x_init，y_init，NED坐标系原点的n和e坐标
|
||||
sys_status，定位系统状态（未初始化和已初始化）
|
||||
* Output : int，初始化标志位
|
||||
result，定位结果结构体
|
||||
**************************************************************************/
|
||||
int pdr_initProc(gnss *pgnss, KfPara *kf, PDR* g_pdr, lct_fs *result);
|
||||
/**************************************************************************
|
||||
* Description : 设置EKF滤波器的Q和R
|
||||
* Input : scane_type, 用户所处场景（开阔和非开阔）
|
||||
nmea_data，NMEA数据结构体
|
||||
g_pdr，PDR结构体
|
||||
sys，运动幅度数据结构体
|
||||
kf，EKF数据结构体
|
||||
**************************************************************************/
|
||||
|
||||
|
||||
/**************************************************************************
|
||||
* Description : 计算GPS轨迹角
|
||||
* Input : gpsPos，GPS历史定位点在ECEF下的三维坐标
|
||||
pgnss，GPS数据结构体
|
||||
* Output : double，轨迹角
|
||||
**************************************************************************/
|
||||
double calGnssTrackHeading(double gpsPos[][3], gnss pgnss);
|
||||
|
||||
|
||||
#endif
|
||||
|
||||
#pragma once
|
||||
|
|
@ -1,85 +0,0 @@
|
|||
#ifndef __DETECTOR_H
|
||||
#define __DETECTOR_H
|
||||
|
||||
#ifdef __cplusplus
|
||||
extern "C" {
|
||||
#endif
|
||||
|
||||
/* Header File Including ------------------------------------------------------------------------ */
|
||||
#include <stdint.h>
|
||||
#include <stdlib.h>
|
||||
#include <time.h>
|
||||
#include "pdr_base.h"
|
||||
#include "buffer.h"
|
||||
#include "pdr_ahrs.h"
|
||||
|
||||
/* Macro Declaration ---------------------------------------------------------------------------- */
|
||||
#define DETECTOR_NO_ERROR 0
|
||||
#define DETECTOR_OUT_OF_MEMORY 1
|
||||
|
||||
#define MAG_BUF_LEN 256
|
||||
#define MAG_LEN 6
|
||||
|
||||
/* Struct Declaration --------------------------------------------------------------------------- */
|
||||
|
||||
|
||||
typedef void (*DETECTOR_update_callback)(DETECTOR *detector);
|
||||
|
||||
/* Global Variable Declaration ------------------------------------------------------------------ */
|
||||
extern BUFFER_SHORT g_acc_frq_buf[3];
|
||||
extern BUFFER_SHORT g_acc_amp_buf[3];
|
||||
extern BUFFER_SHORT g_gyr_frq_buf[3];
|
||||
extern BUFFER_SHORT g_gyr_amp_buf[3];
|
||||
|
||||
/* Function Declaration ------------------------------------------------------------------------- */
|
||||
|
||||
/**---------------------------------------------------------------------
|
||||
* Function : pdr_getDetector
|
||||
* Description : 获取PDR运动分类器的结构体指针
|
||||
* Date : 2020/02/16 logzhan &
|
||||
*
|
||||
*---------------------------------------------------------------------**/
|
||||
DETECTOR *pdr_getDetector(void);
|
||||
|
||||
/**---------------------------------------------------------------------
|
||||
* Function : pdr_initDetector
|
||||
* Description : 初始化运动分类器，识别用户处于哪一种运动模式
|
||||
* Date : 2020/02/16 logzhan & logzhan
|
||||
*---------------------------------------------------------------------**/
|
||||
DETECTOR* pdr_initDetector(void);
|
||||
|
||||
/**---------------------------------------------------------------------
|
||||
* Function : pdr_resetDetector
|
||||
* Description : 重置PDR运动分类器
|
||||
* Date : 2020/02/16 logzhan & logzhan
|
||||
*---------------------------------------------------------------------**/
|
||||
void pdr_resetDetector(void);
|
||||
|
||||
/**---------------------------------------------------------------------
|
||||
* Function : pdr_motionTypeDetect
|
||||
* Description : pdr运动类型检测
|
||||
* Date : 2020/7/20
|
||||
*
|
||||
*---------------------------------------------------------------------**/
|
||||
int pdr_motionTypeDetect(void);
|
||||
|
||||
/**---------------------------------------------------------------------
|
||||
* Function : predict
|
||||
* Description : 判断手机携带方式（静止、手持、摆手等）
|
||||
* Date : 2020/7/20 logzhan
|
||||
*---------------------------------------------------------------------**/
|
||||
int pdr_detectorPredict(float* feature);
|
||||
|
||||
void mag_calibration(IMU *imu);
|
||||
void mag_m_trans(float a[MAG_BUF_LEN][3], float r[3][MAG_BUF_LEN]);
|
||||
void mag_min(float a[MAG_BUF_LEN], float *mag_min);
|
||||
void mag_max(float a[MAG_BUF_LEN], float *mag_max);
|
||||
void mag_rand(float r[6]);
|
||||
|
||||
void updateDetectorImu(imu* imu);
|
||||
|
||||
#ifdef __cplusplus
|
||||
}
|
||||
#endif
|
||||
|
||||
#endif
|
||||
|
|
@ -1,63 +0,0 @@
|
|||
/******************** (C) COPYRIGHT 2020 Geek************************************
|
||||
* File Name : pdr_kalman.h
|
||||
* Department : Sensor Algorithm Team
|
||||
* Current Version : V2.0(compare QCOM SAP 5.0)
|
||||
* Author : logzhan
|
||||
* Date of Issued : 2020.7.3
|
||||
* Comments : PDR 卡尔曼滤波器头文件函数声明以及相关结构体定义
|
||||
********************************************************************************/
|
||||
#ifndef _PDR_KALMAN_H_
|
||||
#define _PDR_KALMAN_H_
|
||||
|
||||
#ifdef __cplusplus
|
||||
extern "C" {
|
||||
#endif
|
||||
|
||||
/**----------------------------------------------------------------------
|
||||
* Function : pdr_initKalman
|
||||
* Description : 初始化卡尔曼滤波器相关
|
||||
* Date : 2020/8/27
|
||||
*
|
||||
*---------------------------------------------------------------------**/
|
||||
void pdr_initKalman(void);
|
||||
|
||||
/**----------------------------------------------------------------------
|
||||
* Function : pdr_ekfStatePredict
|
||||
* Description : pdr卡尔曼滤波器的状态预测方程
|
||||
* Date : 2020/7/22 logzhan
|
||||
*---------------------------------------------------------------------**/
|
||||
void pdr_ekfStatePredict(KfPara* kf, double step_length, PDR* g_pdr, int step);
|
||||
|
||||
/**----------------------------------------------------------------------
|
||||
* Function : getQRValue
|
||||
* Description : 调整卡尔曼滤波噪声矩阵
|
||||
* scane_type, 用户所处场景（开阔和非开阔）
|
||||
* nmea_data，NMEA数据结构体
|
||||
* g_pdr，PDR结构体
|
||||
* sys，运动幅度数据结构体
|
||||
* kf，EKF数据结构体
|
||||
* Date : 2020/07/09 logzhan
|
||||
*---------------------------------------------------------------------**/
|
||||
void calEkfQRMatrix(lct_nmea* nmea_data, PDR* g_pdr, classifer* sys, KfPara* kf);
|
||||
|
||||
/**----------------------------------------------------------------------
|
||||
* Function : pdr_ekfStateUpdate
|
||||
* Description : pdr卡尔曼滤波器的状态更新方程
|
||||
* Date : 2020/7/22 logzhan
|
||||
*---------------------------------------------------------------------**/
|
||||
void pdr_ekfStateUpdate(KfPara* kf, gnss* pgnss, classifer* sys, PDR* g_pdr,
|
||||
int scene_type);
|
||||
|
||||
/**----------------------------------------------------------------------
|
||||
* Function : pdr_gnssInsLocFusion
|
||||
* Description : PDR的GNSS和INS融合定位
|
||||
* Date : 2020/07/09 logzhan
|
||||
*---------------------------------------------------------------------**/
|
||||
void pdr_gnssInsLocFusion(gnss* pgnss, PDR* g_pdr, classifer* sys, double yaw_bias,
|
||||
KfPara* kf, lct_fs* res);
|
||||
|
||||
#ifdef __cplusplus
|
||||
}
|
||||
#endif
|
||||
|
||||
#endif
|
||||
|
|
@ -1,99 +0,0 @@
|
|||
#ifndef __PDR_LOCATION_H__
|
||||
#define __PDR_LOCATION_H__
|
||||
|
||||
#ifdef __cplusplus
|
||||
extern "C" {
|
||||
#endif
|
||||
|
||||
#include "pdr_base.h"
|
||||
|
||||
#define HIST_GPS_NUM 8
|
||||
#define ACCURACY_THRES 0.6f // 精度阈值
|
||||
#define YAW_THRES 10.0f // Yaw角范围阈值
|
||||
|
||||
|
||||
/**---------------------------------------------------------------------
|
||||
* Function : pdr_navSysInit
|
||||
* Description : PDR导航系统初始化
|
||||
* Date : 2020/2/1
|
||||
*
|
||||
*
|
||||
*
|
||||
*
|
||||
*
|
||||
*
|
||||
*---------------------------------------------------------------------**/
|
||||
void NavSys_Init(void);
|
||||
|
||||
int pdr_insLocation(imu *ss_data, KfPara *kf);
|
||||
|
||||
|
||||
/**----------------------------------------------------------------------
|
||||
* Function : pdr_noGpsPredict
|
||||
* Description : 在没有gps信息时预测GPS位置，最多预测10个点
|
||||
* Date : 2020/07/08 logzhan
|
||||
*---------------------------------------------------------------------**/
|
||||
void pdr_noGpsPredict(KfPara* kf, lct_fs* result, PDR* g_pdr);
|
||||
|
||||
/**----------------------------------------------------------------------
|
||||
* Function : pdr_nmea2Gnss
|
||||
* Description : nmea数据结构转gnss数据
|
||||
* Date : 2020/07/08 logzhan
|
||||
*---------------------------------------------------------------------**/
|
||||
void pdr_nmea2Gnss(lct_nmea* nmea_data, gnss* pgnss);
|
||||
|
||||
|
||||
/**----------------------------------------------------------------------
|
||||
* Function : pdr_detectFixMode
|
||||
* Description : 检测当前PDR处于的模式，如果是车载和静止模式,根据情况选择输出
|
||||
* 原始GPS或者不输出
|
||||
* Date : 2020/07/08 logzhan
|
||||
* 2020/02/08 logzhan : 修改-1为INVAILD_GPS_YAW，提高
|
||||
* 代码的可读性
|
||||
*---------------------------------------------------------------------**/
|
||||
int pdr_detectFixMode(gnss* pgnss, KfPara* kf, PDR* g_pdr, lct_fs* result);
|
||||
|
||||
/**----------------------------------------------------------------------
|
||||
* Function : pdr_getGpsHeading
|
||||
* Description : 获取GPS偏航角
|
||||
* Date : 2020/07/08 logzhan
|
||||
*---------------------------------------------------------------------**/
|
||||
double pdr_getGpsHeading(gnss* pgnss);
|
||||
|
||||
/**----------------------------------------------------------------------
|
||||
* Function : calPdrHeadingOffset
|
||||
* Description : 利用GPS信号较好时的偏航角修正磁力计计算方向键的偏移
|
||||
* Date : 2020/07/08 logzhan
|
||||
*---------------------------------------------------------------------**/
|
||||
void calPdrHeadingOffset(lct_nmea* nmea_data, PDR* p_pdr);
|
||||
|
||||
/**---------------------------------------------------------------------
|
||||
* Function : pdr_resetSysStatus
|
||||
* Description : PDS 重设系统状态
|
||||
* Date : 2020/2/1 logzhan
|
||||
*---------------------------------------------------------------------**/
|
||||
void pdr_resetSysStatus(KfPara* kf);
|
||||
|
||||
/**----------------------------------------------------------------------
|
||||
* Function : pdr_gnssInsLocation
|
||||
* Description : PDR GPS融合INS惯性定位
|
||||
* Date : 2021/01/29 logzhan
|
||||
*---------------------------------------------------------------------**/
|
||||
int pdr_gnssInsLocation(lct_nmea* nmea_data, KfPara* kf, lct_fs* result);
|
||||
|
||||
/**---------------------------------------------------------------------
|
||||
* Function : pdr_initGnssInfo
|
||||
* Description : PDS GNSS定位信息初始化
|
||||
* Date : 2020/2/1 logzhan
|
||||
*---------------------------------------------------------------------**/
|
||||
void pdr_initGnssInfo(void);
|
||||
|
||||
void pdr_gnssUpdate(gnss* gps, lct_nmea* nmea);
|
||||
|
||||
|
||||
#ifdef __cplusplus
|
||||
}
|
||||
#endif
|
||||
|
||||
#endif
|
||||
|
||||
|
|
@ -1,85 +0,0 @@
|
|||
/******************** (C) COPYRIGHT 2020 Geek************************************
|
||||
* File Name : pdr_main.h
|
||||
* Department : Sensor Algorithm Team
|
||||
* Current Version : V2.0
|
||||
* Author :
|
||||
|
||||
& yuanlin@vivo.cm
|
||||
* Date of Issued : 2020.7.18
|
||||
* Comments : PDR导航算法平台配置文件
|
||||
********************************************************************************/
|
||||
#ifdef __cplusplus
|
||||
extern "C" {
|
||||
#endif
|
||||
|
||||
#ifndef _PDR_MAIN_H_
|
||||
#define _PDR_MAIN_H_
|
||||
#include <string>
|
||||
|
||||
using namespace std;
|
||||
|
||||
#define PATH_MAX 256
|
||||
#define TRACK_MAX 100000
|
||||
|
||||
typedef struct LatLngd {
|
||||
double lat;
|
||||
double lon;
|
||||
double heading;
|
||||
double hdop;
|
||||
double accuracy;
|
||||
double vel;
|
||||
double time;
|
||||
int motionType;
|
||||
} LatLngd;
|
||||
|
||||
typedef struct ResultTracks {
|
||||
LatLngd gpsTrack[TRACK_MAX];
|
||||
LatLngd pdrTrack[TRACK_MAX];
|
||||
int gpsLen;
|
||||
int pdrLen;
|
||||
}ResultTracks;
|
||||
|
||||
/**----------------------------------------------------------------------
|
||||
* Function : pdr_writeKml
|
||||
* Description : 将pdr算法输出的gps和pdr轨迹写为kml形式
|
||||
* path : kml文件的输出文件路径
|
||||
* name : kml文件主体名称
|
||||
* postfix ：在主体名称后面添加的后缀，用于区分类型或者版本
|
||||
* Date : 2020/11/1 logzhan
|
||||
*---------------------------------------------------------------------**/
|
||||
void KmlWrite(string path, string name, string postfix);
|
||||
|
||||
/**----------------------------------------------------------------------
|
||||
* Function : getSimulateFileFp
|
||||
* Description : 给定文件名，以及文件路径，获取仿真文件的文件指针
|
||||
* Date : 2021/01/25 logzhan
|
||||
*---------------------------------------------------------------------**/
|
||||
FILE* getSimulateFile(FILE* catalogFp, string path_file, string& fileHead);
|
||||
|
||||
/**----------------------------------------------------------------------
|
||||
* Function : gpsYaw2GoogleYaw
|
||||
* Description : 为了让kml显示的角度方位正常，需要把0-360顺时针旋转的Yaw转换
|
||||
* 为谷歌支持的Yaw规则
|
||||
* Date : 2021/01/25 logzhan
|
||||
*---------------------------------------------------------------------**/
|
||||
double gpsYaw2GoogleYaw(double heading);
|
||||
|
||||
/**----------------------------------------------------------------------
|
||||
* Function : motionType2Str
|
||||
* Description : 把用户运动类型转换为字符串输出
|
||||
* Date : 2020/8/3 logzhan
|
||||
*---------------------------------------------------------------------**/
|
||||
const char* motionType2Str(int type);
|
||||
|
||||
/**----------------------------------------------------------------------
|
||||
* Function : updateResTrack
|
||||
* Description : 更新新输出的结果轨迹，包含GPS轨迹和PDR轨迹
|
||||
* Date : 2021/01/25 logzhan
|
||||
*---------------------------------------------------------------------**/
|
||||
void updateResTrack(ResultTracks& resTrack, lct_fs& lctfs);
|
||||
|
||||
#endif
|
||||
|
||||
#ifdef __cplusplus
|
||||
}
|
||||
#endif
|
||||
|
|
@ -1,70 +0,0 @@
|
|||
#ifdef __cplusplus
|
||||
extern "C" {
|
||||
#endif
|
||||
|
||||
#ifndef _PDR_MATRIX_H_
|
||||
#define _PDR_MATRIX_H_
|
||||
|
||||
#define N 4
|
||||
|
||||
/**---------------------------------------------------------------------
|
||||
* Function : MatrixTrans
|
||||
* Description : 矩阵转置
|
||||
* Date : 2022/09/14 logzhan
|
||||
*---------------------------------------------------------------------**/
|
||||
void MatrixTrans(double a[N][N], double r[N][N]);
|
||||
|
||||
/**---------------------------------------------------------------------
|
||||
* Function : VecMatMultiply
|
||||
* Description : 向量和矩阵相乘 r = b * a
|
||||
* Date : 2022/09/14 logzhan
|
||||
*---------------------------------------------------------------------**/
|
||||
void VecMatMultiply(double a[N], double b[N][N], double r[N]);
|
||||
|
||||
/**---------------------------------------------------------------------
|
||||
* Function : MatrixMultiply
|
||||
* Description : 矩阵和矩阵相乘 r = a * b
|
||||
* Date : 2022/09/14 logzhan
|
||||
*---------------------------------------------------------------------**/
|
||||
void MatrixMultiply(double a[N][N], double b[N][N], double r[N][N]);
|
||||
|
||||
/**---------------------------------------------------------------------
|
||||
* Function : MatrixAdd
|
||||
* Description : 矩阵和矩阵相加 r = a + b, 注意这个函数支持a = a + b
|
||||
* Date : 2022/09/14 logzhan
|
||||
*---------------------------------------------------------------------**/
|
||||
void MatrixAdd(double a[N][N], double b[N][N], double r[N][N]);
|
||||
|
||||
/**---------------------------------------------------------------------
|
||||
* Function : VectorAdd
|
||||
* Description : 向量和向量相加 r = a + b, 注意这个函数支持a = a + b
|
||||
* Date : 2022/09/14 logzhan
|
||||
*---------------------------------------------------------------------**/
|
||||
void VectorAdd(double a[N], double b[N], double r[N]);
|
||||
|
||||
/**---------------------------------------------------------------------
|
||||
* Function : MatrixSub
|
||||
* Description : 矩阵和矩阵相减 r = a - b, 注意这个函数支持a = a - b
|
||||
* Date : 2022/09/14 logzhan
|
||||
*---------------------------------------------------------------------**/
|
||||
void MatrixSub(double a[N][N], double b[N][N], double r[N][N]);
|
||||
|
||||
/**---------------------------------------------------------------------
|
||||
* Function : VectorSub
|
||||
* Description : 向量和向量相减 r = a - b, 注意这个函数支持a = a - b
|
||||
* Date : 2022/09/14 logzhan
|
||||
*---------------------------------------------------------------------**/
|
||||
void VectorSub(double a[N], double b[N], double r[N]);
|
||||
|
||||
/**---------------------------------------------------------------------
|
||||
* Function : MatrixInverse
|
||||
* Description : 求矩阵的逆矩阵
|
||||
* Date : 2022/09/14 logzhan
|
||||
*---------------------------------------------------------------------**/
|
||||
void MatrixInverse(double(*a)[N], double(*a_inv)[N]);
|
||||
|
||||
#endif
|
||||
|
||||
#ifdef __cplusplus
|
||||
}
|
||||
#endif
|
||||
|
|
@ -1,314 +0,0 @@
|
|||
/******************** (C) COPYRIGHT 2021 Geek************************************
|
||||
* File Name : pdr_sensor.h
|
||||
* Department : Sensor Algorithm Team
|
||||
* Current Version : V2.0
|
||||
* Author : logzhan
|
||||
* Date of Issued : 2021.02.01
|
||||
* Comments : PDR算法传感器基本定义
|
||||
********************************************************************************/
|
||||
#ifndef _SENSOR_H_
|
||||
#define _SENSOR_H_
|
||||
|
||||
#ifdef __cplusplus
|
||||
extern "C" {
|
||||
#endif
|
||||
|
||||
#include <stdint.h>
|
||||
|
||||
/* Macro Definition ----------------------------------------------------------*/
|
||||
#define IMU_SENSOR_AXIS 3
|
||||
|
||||
|
||||
#ifdef _WIN32
|
||||
#define PDR_LOGD printf
|
||||
#define PDR_LOGE printf
|
||||
#else
|
||||
#include <android/log.h>
|
||||
#define LOG_TAG "Loc_PDR"
|
||||
#define PDR_LOGD(...) __android_log_print(ANDROID_LOG_DEBUG, LOG_TAG, __VA_ARGS__)
|
||||
#define PDR_LOGE(...) __android_log_print(ANDROID_LOG_ERROR, LOG_TAG, __VA_ARGS__)
|
||||
#endif
|
||||
|
||||
#define ADD_POINT_NUM 10
|
||||
|
||||
#define GGA_STR "GGA"
|
||||
#define RMC_STR "RMC"
|
||||
#define GSA_STR "GSA"
|
||||
#define GSV_STR "GSV"
|
||||
|
||||
#define GNGGA_STR "$GNGGA"
|
||||
#define GPGGA_STR "$GPGGA"
|
||||
#define GNRMC_STR "$GNRMC"
|
||||
#define GPRMC_STR "$GPRMC"
|
||||
#define GNGSA_STR "$GNGSA"
|
||||
#define GPGSA_STR "$GPGSA"
|
||||
#define GAGSA_STR "$GAGSA"
|
||||
#define GLGSA_STR "$GLGSA"
|
||||
#define GPGSV_STR "$GPGSV"
|
||||
#define GAGSV_STR "$GAGSV"
|
||||
#define GLGSV_STR "$GLGSV"
|
||||
|
||||
#define NORTH "N"
|
||||
#define SOUTH "S"
|
||||
#define EAST "E"
|
||||
#define WEST "W"
|
||||
#define ITEM_INVALID (-1)
|
||||
#define INVAILD_GPS_YAW (-1) // 无效GPS航向角定义
|
||||
/* Struct Definition ---------------------------------------------------------*/
|
||||
typedef enum {
|
||||
SENSOR_MIN,
|
||||
SENSOR_ACC = 1,
|
||||
SENSOR_MAG = 2,
|
||||
SENSOR_GYRO = 4,
|
||||
SENSOR_LIGHT = 5,
|
||||
SENSOR_PROXIMITY = 8,
|
||||
SENSOR_GAME_ROTATION_VECTOR = 15,
|
||||
SENSOR_LOCATION_STATELLITE = 70,
|
||||
SENSOR_LOCATION_NETWORK = 71,
|
||||
SENSOR_LOCATION_FUSION = 72,
|
||||
SENSOR_LOCATION_NMEA = 73,
|
||||
SENSOR_LOCATION_NMEA_GNGGA = 74,
|
||||
SENSOR_LOCATION_NMEA_GPGGA = 75,
|
||||
SENSOR_LOCATION_NMEA_GNRMC = 76,
|
||||
SENSOR_LOCATION_NMEA_GPRMC = 77,
|
||||
SENSOR_LOCATION_NMEA_GNGSA = 78,
|
||||
SENSOR_LOCATION_NMEA_GPGSA = 79,
|
||||
SENSOR_LOCATION_NMEA_GPGSV = 80,
|
||||
SENSOR_LOCATION_NMEA_GLGSV = 81,
|
||||
SENSOR_LOCATION_NMEA_GAGSV = 82,
|
||||
SENSOR_LOCATION_NMEA_GLGSA = 83,
|
||||
SENSOR_LOCATION_NMEA_GAGSA = 84,
|
||||
SENSOR_MAX,
|
||||
} sensor;
|
||||
|
||||
typedef enum {
|
||||
POSITIONING_N = 0,
|
||||
POSITIONING_Y = 1,
|
||||
}lct_status;
|
||||
|
||||
typedef enum {
|
||||
POSITIONING_MODE_A = 0,
|
||||
POSITIONING_MODE_M = 1,
|
||||
}lct_mode;
|
||||
|
||||
typedef enum PDR_Mode {
|
||||
RAW_GPS = 1,
|
||||
STOP = 2,
|
||||
WALK_PDR = 3,
|
||||
WALK_GPS = 4,
|
||||
CAR = 5
|
||||
} PDR_Mode;
|
||||
|
||||
typedef enum DIR_Mode {
|
||||
GPS_DATASHEET = 0,
|
||||
IMU_PCA = 1,
|
||||
DELTA_LOCATION = 2
|
||||
} DIR_Mode;
|
||||
|
||||
/*for GSA*/
|
||||
typedef enum {
|
||||
PSTN_GSA_N = 1,
|
||||
PSTN_GSA_Y_2D = 2,
|
||||
PSTN_GSA_Y_3D = 3,
|
||||
}lct_type;
|
||||
|
||||
typedef enum {
|
||||
LATITUDE_N = 0,
|
||||
LATITUDE_S = 1,
|
||||
LONGITUDINAL_E = 2,
|
||||
LONGITUDINAL_W = 3,
|
||||
}LL_NSEW;
|
||||
|
||||
typedef enum {
|
||||
MAG_E = 0,
|
||||
MAG_W = 1,
|
||||
}mag_heading;
|
||||
|
||||
typedef enum {
|
||||
MODE_A = 0,
|
||||
MODE_D = 1,
|
||||
MODE_E = 2,
|
||||
MODE_N = 3,
|
||||
}gnrmc_mode;
|
||||
|
||||
typedef enum {
|
||||
invalid = 0,
|
||||
fix = 1,
|
||||
dgps_fix = 2,
|
||||
invalid_pps = 3,
|
||||
rtk_fix = 4,
|
||||
rtk_float = 5,
|
||||
estimating = 6,
|
||||
}gngga_quality;
|
||||
|
||||
typedef struct sensor_agm {
|
||||
unsigned char update;
|
||||
int type;
|
||||
double time; //ms
|
||||
float s[IMU_SENSOR_AXIS];
|
||||
}sensor_agm;
|
||||
|
||||
typedef struct imu {
|
||||
sensor_agm acc;
|
||||
sensor_agm gyr;
|
||||
sensor_agm mag;
|
||||
}imu;
|
||||
|
||||
typedef struct lct_nmea_rmc {
|
||||
unsigned char update;
|
||||
char rmc_check_sum[9];
|
||||
sensor type;
|
||||
lct_status status;
|
||||
LL_NSEW rmc_latitude_ns;
|
||||
LL_NSEW longitudinal_ew;
|
||||
mag_heading mag_h;
|
||||
gnrmc_mode mode;
|
||||
float speed;
|
||||
float cog;
|
||||
float utc_ddmmyy;
|
||||
float magnetic;
|
||||
double rmc_utc;
|
||||
double latitude;
|
||||
double longitudinal;
|
||||
double time; //ms
|
||||
}lct_nmea_rmc;
|
||||
|
||||
typedef struct lct_nmea_gga {
|
||||
unsigned char update;
|
||||
char gga_check_sum[9];
|
||||
sensor type;
|
||||
LL_NSEW gga_latitude_ns;
|
||||
LL_NSEW longitudinal_ew;
|
||||
gngga_quality status;
|
||||
int satellites_nb;
|
||||
int unit_a;
|
||||
int unit_h;
|
||||
int dgps;
|
||||
float hdop;
|
||||
float altitude;
|
||||
float height;
|
||||
double gga_utc;
|
||||
double latitude;
|
||||
double longitudinal;
|
||||
double time; //ms
|
||||
}lct_nmea_gga;
|
||||
|
||||
typedef struct lct_nmea_gsa {
|
||||
unsigned char update;
|
||||
char check_sum[9];
|
||||
sensor type;
|
||||
lct_mode mode;
|
||||
lct_type p_type;
|
||||
int prnflg;
|
||||
int prn[12];
|
||||
float pdop;
|
||||
float hdop;
|
||||
float vdop;
|
||||
double time; //ms
|
||||
}
|
||||
lct_nmea_gsa;
|
||||
|
||||
typedef struct satellite_status {
|
||||
int prn;
|
||||
int elevation; // elevation >10
|
||||
int azimuth;
|
||||
int snr; //snr >25 good
|
||||
}satellite_status;
|
||||
|
||||
typedef struct lct_nmea_gsv {
|
||||
unsigned char update;
|
||||
sensor type;
|
||||
int sentences;
|
||||
int sentence_number;
|
||||
int satellites;
|
||||
int satellite_number;
|
||||
satellite_status satellites_data[20];
|
||||
double time; //ms
|
||||
}lct_nmea_gsv;
|
||||
|
||||
typedef struct loc_accuracy{
|
||||
unsigned char update;
|
||||
int status; //-1,:Status Unknown; 0: Out of Service; 1: Temporarily Unavailable; 2: Available
|
||||
float err; // m
|
||||
double time;
|
||||
}loc_accuracy;
|
||||
|
||||
typedef struct lct_nmea {
|
||||
unsigned char update;
|
||||
double minTime;
|
||||
double maxTime;
|
||||
lct_nmea_gga gga;
|
||||
lct_nmea_rmc rmc[2]; // 0 - GNRMC, 1 - GPRMC
|
||||
lct_nmea_gsa gsa[3]; // 0 - GPGSA, 1 - GLGSA, 2 - GAGSA
|
||||
lct_nmea_gsv gsv[3]; // 0 - GPGSV, 1 - GLGSV, 2 - GAGSV
|
||||
loc_accuracy accuracy;
|
||||
}lct_nmea;
|
||||
|
||||
typedef struct lct_fs {
|
||||
LL_NSEW latitude_ns;
|
||||
LL_NSEW longitudinal_ew;
|
||||
double latitude;
|
||||
double longitudinal;
|
||||
double gpsLat;
|
||||
double gpsLon;
|
||||
double gpsHeading;
|
||||
double pdrHeading;
|
||||
double hdop;
|
||||
double gpsSpeed;
|
||||
double accuracy;
|
||||
double time;
|
||||
double yaw;
|
||||
double lambda;
|
||||
double last_lat;
|
||||
double last_lon;
|
||||
unsigned long step;
|
||||
uint8_t motionType;
|
||||
}lct_fs;
|
||||
|
||||
typedef struct gnss {
|
||||
LL_NSEW lat_ns;
|
||||
LL_NSEW lon_ew;
|
||||
int minElevation;
|
||||
int satellites_num; // 当前卫星数量
|
||||
int quality;
|
||||
float yaw; // GNSS 航向角
|
||||
float vel; // GNSS 速度
|
||||
float HDOP; // GNSS 水平精度因子
|
||||
float error; // 当前GPS的Accuracy
|
||||
float lastError; // 上一次GPS的Accuracy
|
||||
int overVelCount; // 速度过快数量统计, 记录速度过快的数量，用于判断手机是否处于车载模式
|
||||
double timestamps;
|
||||
double lat;
|
||||
double lon;
|
||||
double xNed;
|
||||
double yNed;
|
||||
}gnss;
|
||||
|
||||
typedef struct IMU {
|
||||
double time;
|
||||
float gyr[3];
|
||||
float acc[3];
|
||||
float mag[3];
|
||||
} IMU;
|
||||
|
||||
typedef struct AHRS {
|
||||
uint8_t status;
|
||||
uint8_t stable; // 当前AHRS算法收敛稳定性
|
||||
float error;
|
||||
float qnb[4];
|
||||
float gravity[3];
|
||||
float x_axis[3];
|
||||
float y_axis[3];
|
||||
float z_axis[3];
|
||||
float kp; // mahony kp比例调节参数
|
||||
float yaw;
|
||||
float pitch;
|
||||
float roll;
|
||||
float insHeading;
|
||||
} AHRS;
|
||||
|
||||
#ifdef __cplusplus
|
||||
}
|
||||
#endif
|
||||
#endif
|
||||
|
||||
|
|
@ -1,158 +0,0 @@
|
|||
#ifndef _PDR_UTIL_H_
|
||||
#define _PDR_UTIL_H_
|
||||
|
||||
#ifdef __cplusplus
|
||||
extern "C" {
|
||||
#endif
|
||||
|
||||
#include <stdio.h>
|
||||
|
||||
|
||||
#define TRAJ_POINT_COUNT 1000
|
||||
#define MIN_POINT_COUNT 15
|
||||
#define OPT_TRAJ_TIMEOUT 1000
|
||||
|
||||
#define FALIED_LINE 0
|
||||
#define IN_LINE 1
|
||||
#define CLOSE_START 2
|
||||
#define CLOSE_END 3
|
||||
|
||||
typedef struct LatLng{
|
||||
double lat;
|
||||
double lon;
|
||||
} LatLng;
|
||||
|
||||
typedef struct TrajSign
|
||||
{
|
||||
int count;
|
||||
double lastTime;
|
||||
double weight;
|
||||
double yaw[TRAJ_POINT_COUNT];
|
||||
double error[TRAJ_POINT_COUNT];
|
||||
double points[TRAJ_POINT_COUNT][2];
|
||||
}TrajSign;
|
||||
|
||||
/**地球相关参数*******************************************************************/
|
||||
typedef struct
|
||||
{
|
||||
double rmh; /* 子午圈曲率半径 */
|
||||
double rnh; /* 卯酉圈曲率半径 */
|
||||
double grav; /* 当地重力加速度 */
|
||||
double lat; /* 当地纬度（rad）*/
|
||||
double wnie[3]; /* 地理系相对惯性系的角速度在导航系分量 */
|
||||
} EarthData_t;
|
||||
|
||||
/**----------------------------------------------------------------------
|
||||
* Function : mean
|
||||
* Description : 对输入double数组求均值
|
||||
* Review : 求均值过程每次都做除法，有优化空间，可以针对求解均值的数值大小采用
|
||||
* 不同函数
|
||||
* Date : 2020/7/4 logzhan
|
||||
*---------------------------------------------------------------------**/
|
||||
double mean(double *x, int n);
|
||||
|
||||
float fmean(float *data, int len);
|
||||
|
||||
float stdf(float* data, int len);
|
||||
|
||||
double meanAngle(double* angle, int len);
|
||||
|
||||
void modAngle(double * angle, double min, double max);
|
||||
|
||||
double pow_i(double num, long long n);
|
||||
|
||||
double pow_f(double num, double m);
|
||||
|
||||
double vivopow(double x, double y);
|
||||
|
||||
void centralization(double *x, double *x_out, int n);
|
||||
|
||||
void getCovMatrix(double *x, double *y, double cov[2][2], int n);
|
||||
|
||||
int Jacobi(double a[][2], double p[][2], int n, double eps, int T);
|
||||
|
||||
void QuaternConj(float _q[], float q[]);
|
||||
|
||||
void quaternProd(float ab[], float a[], float b[]);
|
||||
|
||||
void WGS842ECEF(double *plla, double *ecef);
|
||||
|
||||
void ECEF2WGS84(double *ecef, double *plla);
|
||||
|
||||
void ECEF2Ned(double *ecef, double *plla, double *ned);
|
||||
|
||||
void NED2ECEF(double *plla, double *ned, double *ecef0, double *ecef);
|
||||
|
||||
void wgs842Ned(double *plla, double *ref_lla, double *ned);
|
||||
|
||||
void ned2Wgs84(double *ref_plla, double *ned, double* plla);
|
||||
|
||||
|
||||
LatLng ProjPointOfLatLng(LatLng point, LatLng linePointA, LatLng linePointB);
|
||||
|
||||
double calDistance(LatLng pointA, LatLng pointB);
|
||||
|
||||
void ProjPointOfLatLng_cir(double point1[], double yaw, double point2[], double result[]);
|
||||
/**----------------------------------------------------------------------
|
||||
* Function : pdr_invSqrt
|
||||
* Description : 计算1/sqrt(x)的快速的算法
|
||||
* Date : 2020/6/30 logzhan
|
||||
*---------------------------------------------------------------------**/
|
||||
float InvSqrt(float x);
|
||||
|
||||
/**---------------------------------------------------------------------
|
||||
* Function : pdr_v3Norm
|
||||
* Description : 三维浮点数向量归一化
|
||||
* Date : 2021/01/26 yuanlin@vivocom &&
|
||||
*
|
||||
*
|
||||
*---------------------------------------------------------------------**/
|
||||
void pdr_v3Norm(float* vx, float* vy, float* vz);
|
||||
|
||||
/**---------------------------------------------------------------------
|
||||
* Function : quatNorm
|
||||
* Description : 四元数归一化
|
||||
* Date : 2021/01/26 yuanlin@vivocom && logzhan
|
||||
* 2021/01/27 logzhan : 优化了代码逻辑，防止当四元数
|
||||
* 全为0时产生的异常情况。
|
||||
*---------------------------------------------------------------------**/
|
||||
void QuatNorm(float* q0, float* q1, float* q2, float* q3);
|
||||
|
||||
/**----------------------------------------------------------------------
|
||||
* Function : pdr_writeCsvStr
|
||||
* Description : 将字符串信息写入文本，主要是对fprintf封装一层
|
||||
* Date : 2020/7/8 logzhan
|
||||
*---------------------------------------------------------------------**/
|
||||
void pdr_WriteCsvStr(FILE* fp_write, char* str);
|
||||
|
||||
/**----------------------------------------------------------------------
|
||||
* Function : pdr_utc2hms
|
||||
* Description : 将UTC时间转换为时分秒
|
||||
* Date : 2020/7/8 logzhan
|
||||
*---------------------------------------------------------------------**/
|
||||
void pdr_utc2hms(double utc, double* h, double* m, double* s);
|
||||
|
||||
/**----------------------------------------------------------------------
|
||||
* Function : qnb2att
|
||||
* Description : 四元数转欧拉角
|
||||
* Date : 2020/7/4 logzhan
|
||||
*---------------------------------------------------------------------**/
|
||||
void Qnb2Att(float* q, double* attitude);
|
||||
|
||||
double CalRadianDifferent(double s_dir, double d_dir);
|
||||
|
||||
/**----------------------------------------------------------------------
|
||||
* Function : pdr_earthParameter
|
||||
* Description : 根据输入纬度返回该纬度地球相关参数
|
||||
* Date : 2020/7/8 logzhan
|
||||
*---------------------------------------------------------------------**/
|
||||
EarthData_t CalEarthParameter(double oriLat);
|
||||
|
||||
int pdr_min(int param_a, int param_b);
|
||||
|
||||
#ifdef __cplusplus
|
||||
}
|
||||
#endif
|
||||
|
||||
#endif
|
||||
|
||||
|
|
@ -1,164 +0,0 @@
|
|||
/*********************************************************************************/
|
||||
/* FILE NAME : scene_recognition.h */
|
||||
/* AUTHOR : Zhang Jingrui, ID: 11082826, Version: V0.1_Beta, Data:2019-03-28 */
|
||||
/* DESCRIPTION: This header file conterns the prototype description of the external */
|
||||
/* interface funtions of the scene recognition module and the definit- */
|
||||
/* ion of related resources. */
|
||||
/* HISTORY : (NONE) */
|
||||
/********************************************************************************/
|
||||
|
||||
#ifndef _PDR_SCENE_RECOGNITION_H_
|
||||
#define _PDR_SCENE_RECOGNITION_H_
|
||||
|
||||
/* Header Files Including -----------------------------------------*/
|
||||
/*=================================================================*/
|
||||
#include "pdr_sensor.h"
|
||||
|
||||
/* Macro Declaration ----------------------------------------------*/
|
||||
/*=================================================================*/
|
||||
|
||||
#define _TIME_BUFFER_SIZE 10 /* Buffer size for the time calculation. */
|
||||
#define _ACCURACY_BUFFER_SIZE 9 /* Accuracy time-based buffer size. */
|
||||
#define _GNSS_INDEX_LEN 103 /* Structural array size in _GnssInfo. */
|
||||
|
||||
/* Extern Variable Declaration ------------------------------------*/
|
||||
/*=================================================================*/
|
||||
//extern int scene_recognition_debug_flag ;
|
||||
//extern int scene_recognition_log_debug_flag ;
|
||||
|
||||
/* Status Declaration ---------------------------------------------*/
|
||||
/*=================================================================*/
|
||||
/* Usage: 场景识别模块初始化、重置操作结果状态标志位 */
|
||||
typedef enum SCENE_INIT_STATE
|
||||
{
|
||||
// 场景识别模块初始化相关状态
|
||||
INIT_SUCCESS = 0,
|
||||
INIT_NOT_PERFORMED,
|
||||
|
||||
INIT_ERROR_MEMORY_OCCUPIED, // 模块资源未提前释放，需先释放资源
|
||||
INIT_ERROR_MEMORY_INIT_FAILED, // 初始化内存申请失败
|
||||
INIT_ERROR_REDUNDANCY, // 模块重复初始化，此时初始化无效。若需要重启模块，可以：
|
||||
// 方法1：先关闭模块，关闭成功后，重新初始化模块。
|
||||
// 方法2：重启模块，效果同方法1。
|
||||
// 场景识别模块重置相关状态(for debug)
|
||||
INIT_RESET_SUCCESS = 0,
|
||||
INIT_RESET_FAILED
|
||||
} SCENE_INIT_STATE;
|
||||
|
||||
/* Usage: 场景分类结果 */
|
||||
typedef enum SCENE_RECOGNITION_RESULT
|
||||
{
|
||||
RECOG_UNKNOWN, // 未能确定场景的状态(默认状态)
|
||||
RECOG_OPEN_AREA, // 场景open-area
|
||||
RECOG_OTHERS, // 除了上述确定场景外的其他场景
|
||||
RECOG_ERROR_PARAM_INCRECT, // 输入参数检查错误
|
||||
RECOG_ERROR_NMEA_INFO_MISS, // 输入参数正确但是内容有误
|
||||
/* 预研检测结果(不稳定) */
|
||||
RECOG_MULTIPATH // 检测多径
|
||||
} SCENE_RECOGNITION_RESULT;
|
||||
|
||||
/* Usage: 场景识别模块资源释放操作结果 */
|
||||
typedef enum SCENE_DESTROY_STATE
|
||||
{
|
||||
DESTROY_SUCCESS = 0,
|
||||
DESTROY_FIALED,
|
||||
DESTROY_ERROR,
|
||||
DESTROY_INVALID // 无效释放操作，表示没有对应的初始化或重置操作
|
||||
} SCENE_DESTROY_STATE;
|
||||
|
||||
|
||||
/* Status Declaration--------------------------------------------*/
|
||||
/*===============================================================*/
|
||||
|
||||
// 描述当前场景识别模块的工作状态
|
||||
typedef enum _SCENE_MODEL_WORK_STATE
|
||||
{
|
||||
MODEL_OFF,
|
||||
MODEL_ON
|
||||
} _SCENE_MODEL_WORK_STATE;
|
||||
|
||||
/* Usage: Identifier of the quality of the satellites' signal source. */
|
||||
typedef enum _SIGNAL_QUALITY
|
||||
{
|
||||
SIG_UNKNOWN,
|
||||
GOOD,
|
||||
BAD
|
||||
} SIGNAL_QUALITY;
|
||||
|
||||
/* Usage: extract and save information from lct_nmea for the module. */
|
||||
/* This is the interface for the lct_nmea. */
|
||||
typedef struct _GnssInfo
|
||||
{
|
||||
int update; // GNSS更新标志位 1：更新 0：不更新
|
||||
double local_timestamp; // 当地时间
|
||||
float accuracy; // GPS 输出的Accuracy参数，Accuracy > 0 当其为负数表示无效
|
||||
int sat_visible_number; // 当前时间可见卫星数量
|
||||
float snr_list[_GNSS_INDEX_LEN]; // 当前时间可见卫星的SNR列表
|
||||
int sat_used_list[_GNSS_INDEX_LEN]; // 当前使用卫星列表
|
||||
} GnssInfo;
|
||||
|
||||
/* Usage: 标记GNSS中的各个卫星系统 */
|
||||
|
||||
/* Struct Declaration -------------------------------------------*/
|
||||
/*===============================================================*/
|
||||
|
||||
|
||||
/* Function Declaration -------------------------------------------*/
|
||||
/*=================================================================*/
|
||||
#ifdef __cplusplus
|
||||
extern "C" {
|
||||
#endif
|
||||
|
||||
/* Function Name: sceneRecognitionInit() */
|
||||
/* Usage: Initialization module for configuring related resources */
|
||||
/* and must be executed once first beforing using the scene */
|
||||
/* recognition module funcition. */
|
||||
/* Param @ (NONE) */
|
||||
/* Return @ init_res: Result state of the initialization process. */
|
||||
SCENE_INIT_STATE initSceneRecognition(void);
|
||||
|
||||
/* Function Name: scene_recognition_reset() */
|
||||
/* Usage: 初始化模块，配置相关资源，在使用模块前必须首先被执行。 */
|
||||
/* Param @ (NONE) */
|
||||
/* Return @ init_res: 重置执行结果。 */
|
||||
SCENE_INIT_STATE scene_recognition_reset(void);
|
||||
|
||||
/**----------------------------------------------------------------------
|
||||
* Function : sceneRecognitionProc
|
||||
* Description : GNSS场景识别处理流程：
|
||||
* 1）将nmea转换为gnss
|
||||
* 2) 通过accuracy、snr、卫星数量分析GNSS信号质量
|
||||
* 3) 返回结果
|
||||
* Input : PDR的nmea结构体
|
||||
* Return : 场景识别结果
|
||||
* Date : 2020/02/18
|
||||
*
|
||||
*
|
||||
*
|
||||
*
|
||||
*
|
||||
*
|
||||
*---------------------------------------------------------------------**/
|
||||
SCENE_RECOGNITION_RESULT sceneRecognitionProc(const lct_nmea* nmea);
|
||||
|
||||
/* Function Name: scene_recognition_destroy() */
|
||||
/* Usage: 场景识别处理结束后的资源释放操作，资源释放后无法继续进行场 */
|
||||
/* 景识别过程。 */
|
||||
/* Param @ (NONE) */
|
||||
/* Return @ destroy_state: 资源释放操作结果。 */
|
||||
SCENE_DESTROY_STATE scene_recognition_destroy(void);
|
||||
|
||||
/**----------------------------------------------------------------------
|
||||
* 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);
|
||||
|
||||
#ifdef __cplusplus
|
||||
}
|
||||
#endif
|
||||
#endif //__SCENE_RECOGNITION_H__
|
||||
|
|
@ -1,177 +0,0 @@
|
|||
#pragma once
|
||||
/******************** (C) COPYRIGHT 2017 Geek************************************
|
||||
* File Name : Steps_Algorithm.h
|
||||
* Department : Sensor Team
|
||||
* Current Version : V1.0
|
||||
* Author : Xiaoyong Li
|
||||
* Date of Issued : 2017.05.8
|
||||
* Comments: step counter algorithm added
|
||||
********************************************************************************
|
||||
* THE PRESENT SOFTWARE WHICH IS FOR GUIDANCE ONLY AIMS AT PROVIDING CUSTOMERS
|
||||
* WITH CODING INFORMATION REGARDING THEIR PRODUCTS IN ORDER FOR REFERENCE OR
|
||||
* EDUCATION. AS A RESULT, Geek SOFTWARE SHALL NOT BE HELD LIABLE FOR ANY
|
||||
* DIRECT, INDIRECT OR CONSEQUENTIAL DAMAGES WITH RESPECT TO ANY CLAIMS ARISING
|
||||
* FROM THE CONTENT OF SUCH SOFTWARE AND/OR THE USE MADE BY CUSTOMERS OF THE
|
||||
* CODING INFORMATION CONTAINED HEREIN IN CONNECTION WITH THEIR PRODUCTS.
|
||||
********************************************************************************
|
||||
* Release Log
|
||||
*
|
||||
* Comments: step counter algorithm released
|
||||
* Version Number : V1.0
|
||||
* Author : Xiaoyong Li
|
||||
* Date of Issued : 2017.05.8
|
||||
*******************************************************************************/
|
||||
|
||||
#ifdef __cplusplus
|
||||
extern "C" {
|
||||
#endif
|
||||
#ifndef _STEPS_ALGORITHM_H_
|
||||
#define _STEPS_ALGORITHM_H_
|
||||
|
||||
#include "pdr_sensor.h"
|
||||
#define ACC_SAMPLE_RATE_100HZ
|
||||
//#define ACC_SAMPLE_RATE_25HZ
|
||||
|
||||
#ifdef ACC_SAMPLE_RATE_25HZ
|
||||
#define DownResampleTimes 1
|
||||
#define ACC_SAMPLE_TIME 40
|
||||
#define SAMPLE_TIME_MIN 36
|
||||
#else
|
||||
#define DOWN_SAMPLE_TIMES 4
|
||||
#define ACC_SAMPLE_TIME 10 //origin acc sampleFrequency is 100 hz
|
||||
#define GYRO_SAMPLE_TIME 10
|
||||
#define SAMPLE_TIME_MIN 9
|
||||
#endif
|
||||
|
||||
#define SampleFrequency 25
|
||||
#define SampleFrequencyFour 100
|
||||
#define SampleFrequencydouble 50
|
||||
#define SampleFrequencyOPF (37.5l)
|
||||
#define SampleFrequencySix 150
|
||||
#define AvergeFilterOrder 5
|
||||
#define AxisNumber 4
|
||||
#define FiltDifNumber 4
|
||||
#define AxisNumberFiltOut 16 //(AxisNumber * FiltDifNumber)
|
||||
#define AxisNumberStepSaveState 20 //(AxisNumberFiltOut + FiltDifNumber)
|
||||
#define AccelerationNumber 39
|
||||
#define ORI_ACC_SAMPLE_RATE 100 //origin acc sampleFrequency is 100 hz
|
||||
#define WIN_LENGTH_AVE_DEVI 50 //
|
||||
#define PI2A (180/3.1415926)
|
||||
//#define NSTOS 1000000000
|
||||
#define MS2S 1000
|
||||
|
||||
|
||||
#define ACC_INSPECT_TIMEWIDTH 200 //origin acc sampleFrequency is 100 hz
|
||||
|
||||
#define WINDOW_LENGTH_MIN2MAX (ACC_INSPECT_TIMEWIDTH/2 - 1) //25hz , 1 second
|
||||
//#define WINDOW_LENGTH_IN_VEHICLE
|
||||
#define STARTSTEPS 8
|
||||
|
||||
/*#define PLATFORM_ANDROID_QCOM_AP*/
|
||||
/*#define PLATFORM_ANDROID_QCOM_ADSP*/
|
||||
#define PLATFORM_PC_WINDOWS
|
||||
//#define PLATFORM_PC_LINUX
|
||||
|
||||
|
||||
#if defined PLATFORM_ANDROID_QCOM_ADSP
|
||||
#define STEPLIB_MEMSET SNS_OS_MEMSET
|
||||
|
||||
#if 1
|
||||
#define SAM_STEP_LIB_NAME "steplib"
|
||||
#define SAM_STEP_LIB_MSG_0(msg) UMSG(MSG_SSID_SNS,DBG_ERROR_PRIO, SAM_STEP_LIB_NAME" - "msg)
|
||||
#define SAM_STEP_LIB_MSG_1(msg,p1) UMSG_1(MSG_SSID_SNS,DBG_ERROR_PRIO, SAM_STEP_LIB_NAME" - "msg,p1)
|
||||
#define SAM_STEP_LIB_MSG_2(msg,p1,p2) UMSG_2(MSG_SSID_SNS,DBG_ERROR_PRIO, SAM_STEP_LIB_NAME" - "msg,p1,p2)
|
||||
#define SAM_STEP_LIB_MSG_3(msg,p1,p2,p3) UMSG_3(MSG_SSID_SNS,DBG_ERROR_PRIO, SAM_STEP_LIB_NAME" - "msg,p1,p2,p3)
|
||||
#define SAM_STEP_LIB_MSG_4(msg,p1,p2,p3,p4) UMSG_4(MSG_SSID_SNS,DBG_ERROR_PRIO, SAM_STEP_LIB_NAME" - "msg,p1,p2,p3,p4)
|
||||
#else
|
||||
#define SAM_STEP_LIB_MSG_0(msg)
|
||||
#define SAM_STEP_LIB_MSG_1(msg,p1)
|
||||
#define SAM_STEP_LIB_MSG_2(msg,p1,p2)
|
||||
#define SAM_STEP_LIB_MSG_3(msg,p1,p2,p3)
|
||||
#define SAM_STEP_LIB_MSG_4(msg,p1,p2,p3,p4)
|
||||
#endif
|
||||
#elif defined PLATFORM_ANDROID_QCOM_AP || defined PLATFORM_PC_WINDOWS || defined PLATFORM_PC_LINUX || defined PLATFORM_ANDROID_MTK || defined PLATFORM_ANDROID_QCOM_SLPI845
|
||||
#define STEPLIB_MEMSET memset
|
||||
#endif
|
||||
#if defined PLATFORM_ANDROID_MTK
|
||||
#define ABS_INT abs_value
|
||||
#else
|
||||
#define ABS_INT abs
|
||||
#endif
|
||||
|
||||
|
||||
typedef struct {
|
||||
unsigned long walkSteps;
|
||||
}stepsRe;
|
||||
|
||||
typedef struct {
|
||||
double walk;
|
||||
double walkRealTime;
|
||||
double nD;
|
||||
}stepsDfine;
|
||||
|
||||
typedef enum {
|
||||
nonSteps = 0,
|
||||
walk,
|
||||
}enumState;
|
||||
|
||||
typedef struct {
|
||||
unsigned char sign;
|
||||
enumState state;
|
||||
unsigned long long stateStartTime;
|
||||
unsigned long long stepsStartTime;
|
||||
}stateNowDfine;
|
||||
|
||||
typedef struct {
|
||||
enumState state;
|
||||
unsigned long long startTimeStamp;
|
||||
}stateDfine;
|
||||
|
||||
typedef struct {
|
||||
int flag;
|
||||
double ax;
|
||||
double ay;
|
||||
double az;
|
||||
}downSample;
|
||||
|
||||
/**---------------------------------------------------------------------
|
||||
* Function : getPedometerLibVersion
|
||||
* Description : 获取计步器版本
|
||||
* Date : 2020/2/20
|
||||
*---------------------------------------------------------------------**/
|
||||
const char* getPedometerLibVersion(void);
|
||||
|
||||
/**---------------------------------------------------------------------
|
||||
* Function : initPedometer
|
||||
* Description : PDR 初始化计步器
|
||||
* Date : 2020/2/20 logzhan
|
||||
*---------------------------------------------------------------------**/
|
||||
void Pedometer_Init(void);
|
||||
|
||||
/**---------------------------------------------------------------------
|
||||
* Function : reinitPedometer
|
||||
* Description : PDR 重新初始化计步器
|
||||
* Date : 2020/2/20 logzhan
|
||||
*---------------------------------------------------------------------**/
|
||||
void reinitPedometer(void);
|
||||
|
||||
/**---------------------------------------------------------------------
|
||||
* Function : updatePedometer
|
||||
* Description : PDR 计步器的加速度输入, 并通过指针返回用户步数
|
||||
* Date : 2020/2/1 logzhan
|
||||
*---------------------------------------------------------------------**/
|
||||
void updatePedometer(imu* ss_data, unsigned long* step);
|
||||
|
||||
void Steps_State_Detect(enumState* state);
|
||||
|
||||
|
||||
/**---------------------------------------------------------------------
|
||||
* Function : detVehicleMode
|
||||
* Description : PDR 检测汽车模式, 如果是检测到汽车模式，那么不计步数
|
||||
* Date : 2020/2/1 logzhan
|
||||
*---------------------------------------------------------------------**/
|
||||
void detVehicleMode(double ax, double ay, double az);
|
||||
#endif
|
||||
#ifdef __cplusplus
|
||||
}
|
||||
#endif
|
||||
|
|
@ -1,202 +0,0 @@
|
|||
<?xml version="1.0" encoding="utf-8"?>
|
||||
<Project DefaultTargets="Build" ToolsVersion="14.0" xmlns="http://schemas.microsoft.com/developer/msbuild/2003">
|
||||
<ItemGroup Label="ProjectConfigurations">
|
||||
<ProjectConfiguration Include="Debug|Win32">
|
||||
<Configuration>Debug</Configuration>
|
||||
<Platform>Win32</Platform>
|
||||
</ProjectConfiguration>
|
||||
<ProjectConfiguration Include="Debug|x64">
|
||||
<Configuration>Debug</Configuration>
|
||||
<Platform>x64</Platform>
|
||||
</ProjectConfiguration>
|
||||
<ProjectConfiguration Include="Release|Win32">
|
||||
<Configuration>Release</Configuration>
|
||||
<Platform>Win32</Platform>
|
||||
</ProjectConfiguration>
|
||||
<ProjectConfiguration Include="Release|x64">
|
||||
<Configuration>Release</Configuration>
|
||||
<Platform>x64</Platform>
|
||||
</ProjectConfiguration>
|
||||
</ItemGroup>
|
||||
<ItemGroup>
|
||||
<ClCompile Include="..\src\buffer.c" />
|
||||
<ClCompile Include="..\src\LapProcess.c" />
|
||||
<ClCompile Include="..\src\m_munkres.c" />
|
||||
<ClCompile Include="..\src\pdr_ahrs.c" />
|
||||
<ClCompile Include="..\src\pdr_api.c" />
|
||||
<ClCompile Include="..\src\pdr_base.c" />
|
||||
<ClCompile Include="..\src\pdr_detector.c" />
|
||||
<ClCompile Include="..\src\pdr_fft.c" />
|
||||
<ClCompile Include="..\src\pdr_filter.c" />
|
||||
<ClCompile Include="..\src\pdr_kalman.c" />
|
||||
<ClCompile Include="..\src\pdr_linearFit.c" />
|
||||
<ClCompile Include="..\src\pdr_location.c" />
|
||||
<ClCompile Include="..\src\pdr_main.cpp" />
|
||||
<ClCompile Include="..\src\pdr_matrix.c" />
|
||||
<ClCompile Include="..\src\pdr_parseData.c" />
|
||||
<ClCompile Include="..\src\pdr_trackSmooth.c" />
|
||||
<ClCompile Include="..\src\pdr_util.c" />
|
||||
<ClCompile Include="..\src\scene_recognition.c" />
|
||||
<ClCompile Include="..\src\steps.c" />
|
||||
</ItemGroup>
|
||||
<ItemGroup>
|
||||
<ClInclude Include="..\include\buffer.h" />
|
||||
<ClInclude Include="..\include\Filter.h" />
|
||||
<ClInclude Include="..\include\LapProcess.h" />
|
||||
<ClInclude Include="..\include\LocationTool.h" />
|
||||
<ClInclude Include="..\include\m_munkres.h" />
|
||||
<ClInclude Include="..\include\parseData.h" />
|
||||
<ClInclude Include="..\include\pdr_ahrs.h" />
|
||||
<ClInclude Include="..\include\pdr_api.h" />
|
||||
<ClInclude Include="..\include\pdr_base.h" />
|
||||
<ClInclude Include="..\include\pdr_detector.h" />
|
||||
<ClInclude Include="..\include\pdr_fft.h" />
|
||||
<ClInclude Include="..\include\pdr_kalman.h" />
|
||||
<ClInclude Include="..\include\pdr_linearFit.h" />
|
||||
<ClInclude Include="..\include\pdr_location.h" />
|
||||
<ClInclude Include="..\include\pdr_main.h" />
|
||||
<ClInclude Include="..\include\pdr_matrix.h" />
|
||||
<ClInclude Include="..\include\pdr_sensor.h" />
|
||||
<ClInclude Include="..\include\pdr_trackSmooth.h" />
|
||||
<ClInclude Include="..\include\pdr_util.h" />
|
||||
<ClInclude Include="..\include\scene_recognition.h" />
|
||||
<ClInclude Include="..\include\steps.h" />
|
||||
</ItemGroup>
|
||||
<PropertyGroup Label="Globals">
|
||||
<ProjectGuid>{28D32532-309F-40EA-9E4A-2D162CC434D2}</ProjectGuid>
|
||||
<Keyword>Win32Proj</Keyword>
|
||||
<RootNamespace>mp4spliter</RootNamespace>
|
||||
<WindowsTargetPlatformVersion>10.0</WindowsTargetPlatformVersion>
|
||||
<ProjectName>PDR</ProjectName>
|
||||
</PropertyGroup>
|
||||
<Import Project="$(VCTargetsPath)\Microsoft.Cpp.Default.props" />
|
||||
<PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Debug|Win32'" Label="Configuration">
|
||||
<ConfigurationType>Application</ConfigurationType>
|
||||
<UseDebugLibraries>true</UseDebugLibraries>
|
||||
<PlatformToolset>v142</PlatformToolset>
|
||||
<CharacterSet>Unicode</CharacterSet>
|
||||
</PropertyGroup>
|
||||
<PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Debug|x64'" Label="Configuration">
|
||||
<ConfigurationType>Application</ConfigurationType>
|
||||
<UseDebugLibraries>true</UseDebugLibraries>
|
||||
<PlatformToolset>v142</PlatformToolset>
|
||||
<CharacterSet>Unicode</CharacterSet>
|
||||
</PropertyGroup>
|
||||
<PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Release|Win32'" Label="Configuration">
|
||||
<ConfigurationType>Application</ConfigurationType>
|
||||
<UseDebugLibraries>false</UseDebugLibraries>
|
||||
<PlatformToolset>v142</PlatformToolset>
|
||||
<WholeProgramOptimization>true</WholeProgramOptimization>
|
||||
<CharacterSet>Unicode</CharacterSet>
|
||||
</PropertyGroup>
|
||||
<PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Release|x64'" Label="Configuration">
|
||||
<ConfigurationType>Application</ConfigurationType>
|
||||
<UseDebugLibraries>false</UseDebugLibraries>
|
||||
<PlatformToolset>v142</PlatformToolset>
|
||||
<WholeProgramOptimization>true</WholeProgramOptimization>
|
||||
<CharacterSet>Unicode</CharacterSet>
|
||||
</PropertyGroup>
|
||||
<Import Project="$(VCTargetsPath)\Microsoft.Cpp.props" />
|
||||
<ImportGroup Label="ExtensionSettings">
|
||||
</ImportGroup>
|
||||
<ImportGroup Label="PropertySheets" Condition="'$(Configuration)|$(Platform)'=='Debug|Win32'">
|
||||
<Import Project="$(UserRootDir)\Microsoft.Cpp.$(Platform).user.props" Condition="exists('$(UserRootDir)\Microsoft.Cpp.$(Platform).user.props')" Label="LocalAppDataPlatform" />
|
||||
</ImportGroup>
|
||||
<ImportGroup Condition="'$(Configuration)|$(Platform)'=='Debug|x64'" Label="PropertySheets">
|
||||
<Import Project="$(UserRootDir)\Microsoft.Cpp.$(Platform).user.props" Condition="exists('$(UserRootDir)\Microsoft.Cpp.$(Platform).user.props')" Label="LocalAppDataPlatform" />
|
||||
</ImportGroup>
|
||||
<ImportGroup Label="PropertySheets" Condition="'$(Configuration)|$(Platform)'=='Release|Win32'">
|
||||
<Import Project="$(UserRootDir)\Microsoft.Cpp.$(Platform).user.props" Condition="exists('$(UserRootDir)\Microsoft.Cpp.$(Platform).user.props')" Label="LocalAppDataPlatform" />
|
||||
</ImportGroup>
|
||||
<ImportGroup Condition="'$(Configuration)|$(Platform)'=='Release|x64'" Label="PropertySheets">
|
||||
<Import Project="$(UserRootDir)\Microsoft.Cpp.$(Platform).user.props" Condition="exists('$(UserRootDir)\Microsoft.Cpp.$(Platform).user.props')" Label="LocalAppDataPlatform" />
|
||||
</ImportGroup>
|
||||
<PropertyGroup Label="UserMacros" />
|
||||
<PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Debug|Win32'">
|
||||
<LinkIncremental>true</LinkIncremental>
|
||||
<IncludePath>..\include\;$(IncludePath)</IncludePath>
|
||||
<ReferencePath>$(ReferencePath)</ReferencePath>
|
||||
</PropertyGroup>
|
||||
<PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Debug|x64'">
|
||||
<LinkIncremental>true</LinkIncremental>
|
||||
<IncludePath>..\3rdparty\Json\;..\3rdparty\onnx\;..\3rdparty\opencv\;..\include\;$(IncludePath)</IncludePath>
|
||||
<ReferencePath>..\3rdparty\Json;$(ReferencePath)</ReferencePath>
|
||||
</PropertyGroup>
|
||||
<PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Release|Win32'">
|
||||
<LinkIncremental>false</LinkIncremental>
|
||||
</PropertyGroup>
|
||||
<PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Release|x64'">
|
||||
<LinkIncremental>false</LinkIncremental>
|
||||
<IncludePath>..\include\;.\3rdparty\libjson\;..\3rdparty\libcurl\;..\3rdparty\opencv\;$(IncludePath)</IncludePath>
|
||||
<LibraryPath>..\3rdparty\onnx\lib\;..\3rdparty\opencv\lib\;..\3rdparty\libcurl\bin\;..\3rdparty\libjson\bin\;$(LibraryPath)</LibraryPath>
|
||||
<ReferencePath>D:\Program Files\OpenSSL-Win64\include;$(ReferencePath)</ReferencePath>
|
||||
</PropertyGroup>
|
||||
<ItemDefinitionGroup Condition="'$(Configuration)|$(Platform)'=='Debug|Win32'">
|
||||
<ClCompile>
|
||||
<PrecompiledHeader>NotUsing</PrecompiledHeader>
|
||||
<WarningLevel>Level3</WarningLevel>
|
||||
<Optimization>Disabled</Optimization>
|
||||
<PreprocessorDefinitions>WIN32;_DEBUG;_CONSOLE;_LIB;_CRT_SECURE_NO_DEPRECATE;%(PreprocessorDefinitions)</PreprocessorDefinitions>
|
||||
<SDLCheck>true</SDLCheck>
|
||||
<BufferSecurityCheck>true</BufferSecurityCheck>
|
||||
</ClCompile>
|
||||
<Link>
|
||||
<SubSystem>Console</SubSystem>
|
||||
<GenerateDebugInformation>true</GenerateDebugInformation>
|
||||
<AdditionalDependencies>%(AdditionalDependencies)</AdditionalDependencies>
|
||||
</Link>
|
||||
</ItemDefinitionGroup>
|
||||
<ItemDefinitionGroup Condition="'$(Configuration)|$(Platform)'=='Debug|x64'">
|
||||
<ClCompile>
|
||||
<PrecompiledHeader>NotUsing</PrecompiledHeader>
|
||||
<WarningLevel>Level3</WarningLevel>
|
||||
<Optimization>Disabled</Optimization>
|
||||
<PreprocessorDefinitions>WIN32;_DEBUG;_CONSOLE;_LIB;_CRT_SECURE_NO_DEPRECATE;%(PreprocessorDefinitions)</PreprocessorDefinitions>
|
||||
<SDLCheck>true</SDLCheck>
|
||||
<BufferSecurityCheck>true</BufferSecurityCheck>
|
||||
</ClCompile>
|
||||
<Link>
|
||||
<SubSystem>Console</SubSystem>
|
||||
<GenerateDebugInformation>true</GenerateDebugInformation>
|
||||
</Link>
|
||||
</ItemDefinitionGroup>
|
||||
<ItemDefinitionGroup Condition="'$(Configuration)|$(Platform)'=='Release|Win32'">
|
||||
<ClCompile>
|
||||
<WarningLevel>Level3</WarningLevel>
|
||||
<PrecompiledHeader>Use</PrecompiledHeader>
|
||||
<Optimization>MaxSpeed</Optimization>
|
||||
<FunctionLevelLinking>true</FunctionLevelLinking>
|
||||
<IntrinsicFunctions>true</IntrinsicFunctions>
|
||||
<PreprocessorDefinitions>WIN32;NDEBUG;_CONSOLE;_LIB;%(PreprocessorDefinitions)</PreprocessorDefinitions>
|
||||
<SDLCheck>true</SDLCheck>
|
||||
</ClCompile>
|
||||
<Link>
|
||||
<SubSystem>Console</SubSystem>
|
||||
<GenerateDebugInformation>true</GenerateDebugInformation>
|
||||
<EnableCOMDATFolding>true</EnableCOMDATFolding>
|
||||
<OptimizeReferences>true</OptimizeReferences>
|
||||
</Link>
|
||||
</ItemDefinitionGroup>
|
||||
<ItemDefinitionGroup Condition="'$(Configuration)|$(Platform)'=='Release|x64'">
|
||||
<ClCompile>
|
||||
<WarningLevel>Level3</WarningLevel>
|
||||
<PrecompiledHeader>NotUsing</PrecompiledHeader>
|
||||
<Optimization>MaxSpeed</Optimization>
|
||||
<FunctionLevelLinking>true</FunctionLevelLinking>
|
||||
<IntrinsicFunctions>true</IntrinsicFunctions>
|
||||
<PreprocessorDefinitions>WIN32;NDEBUG;_CONSOLE;_LIB;_CRT_SECURE_NO_DEPRECATE;%(PreprocessorDefinitions)</PreprocessorDefinitions>
|
||||
<SDLCheck>true</SDLCheck>
|
||||
</ClCompile>
|
||||
<Link>
|
||||
<SubSystem>Console</SubSystem>
|
||||
<GenerateDebugInformation>true</GenerateDebugInformation>
|
||||
<EnableCOMDATFolding>true</EnableCOMDATFolding>
|
||||
<OptimizeReferences>true</OptimizeReferences>
|
||||
<AdditionalDependencies>%(AdditionalDependencies)</AdditionalDependencies>
|
||||
<LinkTimeCodeGeneration>UseLinkTimeCodeGeneration</LinkTimeCodeGeneration>
|
||||
</Link>
|
||||
</ItemDefinitionGroup>
|
||||
<Import Project="$(VCTargetsPath)\Microsoft.Cpp.targets" />
|
||||
<ImportGroup Label="ExtensionTargets">
|
||||
</ImportGroup>
|
||||
</Project>
|
||||
|
|
@ -1,144 +0,0 @@
|
|||
<?xml version="1.0" encoding="utf-8"?>
|
||||
<Project ToolsVersion="4.0" xmlns="http://schemas.microsoft.com/developer/msbuild/2003">
|
||||
<ItemGroup>
|
||||
<Filter Include="src">
|
||||
<UniqueIdentifier>{7c7c488d-9b32-4de4-9ec8-b4bd38c3e7b0}</UniqueIdentifier>
|
||||
</Filter>
|
||||
<Filter Include="inc">
|
||||
<UniqueIdentifier>{a09dc878-ed5c-47e4-bdcc-c3e2c8f05882}</UniqueIdentifier>
|
||||
</Filter>
|
||||
<Filter Include="src\Utils">
|
||||
<UniqueIdentifier>{1d99ec5e-847f-4166-8f79-ddb0473a04dd}</UniqueIdentifier>
|
||||
</Filter>
|
||||
<Filter Include="src\Math">
|
||||
<UniqueIdentifier>{bd2be68d-d6d0-4b04-97d9-2ac99be07afe}</UniqueIdentifier>
|
||||
</Filter>
|
||||
<Filter Include="src\Location">
|
||||
<UniqueIdentifier>{74391251-b6de-45ec-88b1-0ee0171e44da}</UniqueIdentifier>
|
||||
</Filter>
|
||||
</ItemGroup>
|
||||
<ItemGroup>
|
||||
<ClCompile Include="..\src\buffer.c">
|
||||
<Filter>src</Filter>
|
||||
</ClCompile>
|
||||
<ClCompile Include="..\src\LapProcess.c">
|
||||
<Filter>src</Filter>
|
||||
</ClCompile>
|
||||
<ClCompile Include="..\src\m_munkres.c">
|
||||
<Filter>src</Filter>
|
||||
</ClCompile>
|
||||
<ClCompile Include="..\src\pdr_ahrs.c">
|
||||
<Filter>src</Filter>
|
||||
</ClCompile>
|
||||
<ClCompile Include="..\src\pdr_api.c">
|
||||
<Filter>src</Filter>
|
||||
</ClCompile>
|
||||
<ClCompile Include="..\src\pdr_base.c">
|
||||
<Filter>src</Filter>
|
||||
</ClCompile>
|
||||
<ClCompile Include="..\src\pdr_detector.c">
|
||||
<Filter>src</Filter>
|
||||
</ClCompile>
|
||||
<ClCompile Include="..\src\pdr_fft.c">
|
||||
<Filter>src</Filter>
|
||||
</ClCompile>
|
||||
<ClCompile Include="..\src\pdr_filter.c">
|
||||
<Filter>src</Filter>
|
||||
</ClCompile>
|
||||
<ClCompile Include="..\src\pdr_linearFit.c">
|
||||
<Filter>src</Filter>
|
||||
</ClCompile>
|
||||
<ClCompile Include="..\src\pdr_location.c">
|
||||
<Filter>src</Filter>
|
||||
</ClCompile>
|
||||
<ClCompile Include="..\src\pdr_main.cpp">
|
||||
<Filter>src</Filter>
|
||||
</ClCompile>
|
||||
<ClCompile Include="..\src\pdr_parseData.c">
|
||||
<Filter>src</Filter>
|
||||
</ClCompile>
|
||||
<ClCompile Include="..\src\pdr_trackSmooth.c">
|
||||
<Filter>src</Filter>
|
||||
</ClCompile>
|
||||
<ClCompile Include="..\src\scene_recognition.c">
|
||||
<Filter>src</Filter>
|
||||
</ClCompile>
|
||||
<ClCompile Include="..\src\steps.c">
|
||||
<Filter>src</Filter>
|
||||
</ClCompile>
|
||||
<ClCompile Include="..\src\pdr_util.c">
|
||||
<Filter>src\Utils</Filter>
|
||||
</ClCompile>
|
||||
<ClCompile Include="..\src\pdr_matrix.c">
|
||||
<Filter>src\Math</Filter>
|
||||
</ClCompile>
|
||||
<ClCompile Include="..\src\pdr_kalman.c">
|
||||
<Filter>src\Location</Filter>
|
||||
</ClCompile>
|
||||
</ItemGroup>
|
||||
<ItemGroup>
|
||||
<ClInclude Include="..\include\buffer.h">
|
||||
<Filter>inc</Filter>
|
||||
</ClInclude>
|
||||
<ClInclude Include="..\include\Filter.h">
|
||||
<Filter>inc</Filter>
|
||||
</ClInclude>
|
||||
<ClInclude Include="..\include\LapProcess.h">
|
||||
<Filter>inc</Filter>
|
||||
</ClInclude>
|
||||
<ClInclude Include="..\include\LocationTool.h">
|
||||
<Filter>inc</Filter>
|
||||
</ClInclude>
|
||||
<ClInclude Include="..\include\m_munkres.h">
|
||||
<Filter>inc</Filter>
|
||||
</ClInclude>
|
||||
<ClInclude Include="..\include\parseData.h">
|
||||
<Filter>inc</Filter>
|
||||
</ClInclude>
|
||||
<ClInclude Include="..\include\pdr_ahrs.h">
|
||||
<Filter>inc</Filter>
|
||||
</ClInclude>
|
||||
<ClInclude Include="..\include\pdr_api.h">
|
||||
<Filter>inc</Filter>
|
||||
</ClInclude>
|
||||
<ClInclude Include="..\include\pdr_base.h">
|
||||
<Filter>inc</Filter>
|
||||
</ClInclude>
|
||||
<ClInclude Include="..\include\pdr_detector.h">
|
||||
<Filter>inc</Filter>
|
||||
</ClInclude>
|
||||
<ClInclude Include="..\include\pdr_fft.h">
|
||||
<Filter>inc</Filter>
|
||||
</ClInclude>
|
||||
<ClInclude Include="..\include\pdr_kalman.h">
|
||||
<Filter>inc</Filter>
|
||||
</ClInclude>
|
||||
<ClInclude Include="..\include\pdr_linearFit.h">
|
||||
<Filter>inc</Filter>
|
||||
</ClInclude>
|
||||
<ClInclude Include="..\include\pdr_location.h">
|
||||
<Filter>inc</Filter>
|
||||
</ClInclude>
|
||||
<ClInclude Include="..\include\pdr_main.h">
|
||||
<Filter>inc</Filter>
|
||||
</ClInclude>
|
||||
<ClInclude Include="..\include\pdr_matrix.h">
|
||||
<Filter>inc</Filter>
|
||||
</ClInclude>
|
||||
<ClInclude Include="..\include\pdr_sensor.h">
|
||||
<Filter>inc</Filter>
|
||||
</ClInclude>
|
||||
<ClInclude Include="..\include\pdr_trackSmooth.h">
|
||||
<Filter>inc</Filter>
|
||||
</ClInclude>
|
||||
<ClInclude Include="..\include\pdr_util.h">
|
||||
<Filter>inc</Filter>
|
||||
</ClInclude>
|
||||
<ClInclude Include="..\include\scene_recognition.h">
|
||||
<Filter>inc</Filter>
|
||||
</ClInclude>
|
||||
<ClInclude Include="..\include\steps.h">
|
||||
<Filter>inc</Filter>
|
||||
</ClInclude>
|
||||
</ItemGroup>
|
||||
</Project>
|
||||
|
|
@ -1,343 +0,0 @@
|
|||
|
||||
/* Header File Including ------------------------------------------------------------------------ */#include <stdint.h>
|
||||
#include <string.h>
|
||||
#include "m_munkres.h"
|
||||
|
||||
//#include "loc_vivo_pdr.h"
|
||||
/* Macro Definition ----------------------------------------------------------------------------- */
|
||||
#define COST(i, j) cost[(i) + (j) * m]
|
||||
#define MARK(i, j) mark[(i) + (j) * m]
|
||||
#define PATH(i, j) path[(i) + (j) * 64]
|
||||
#define CLR_ALL(flag) (*flag##_cover = 0)
|
||||
#define SET_FLG(flag, index) (*flag##_cover |= (1 << (index)))
|
||||
#define CLR_FLG(flag, index) (*flag##_cover &= ~(1 << (index)))
|
||||
#define TST_FLG(flag, index) (*flag##_cover & (1 << (index)))
|
||||
|
||||
/* Global Variable Definition ------------------------------------------------------------------- */
|
||||
static float g_cost[64];
|
||||
static int g_mark[64];
|
||||
static uint8_t g_row_cover;
|
||||
static uint8_t g_col_cover;
|
||||
|
||||
|
||||
|
||||
/* Function Declaration ------------------------------------------------------------------------- */
|
||||
static int step_one(float *cost, int m, int n);
|
||||
static int step_two(float *cost, int *mark, int m, int n, uint8_t *col_cover);
|
||||
static int step_three(int *mark, int m, int n, uint8_t *col_cover);
|
||||
static int step_four(float *cost, int *mark, int m, int n, uint8_t *row_cover, uint8_t *col_cover, int *path_start_i, int *path_start_j);
|
||||
static int step_five(int *mark, int m, int n, uint8_t *row_cover, uint8_t *col_cover, int path_start_i, int path_start_j);
|
||||
static int step_six(float *cost, int m, int n, uint8_t *row_cover, uint8_t *col_cover);
|
||||
|
||||
#ifdef __MATLAB_DBG
|
||||
void display_cost_matrix_and_cover(float *cost, int m, int n, uint8_t row_cover, uint8_t col_cover);
|
||||
void display_mark_matrix_and_cover(int *mark, int m, int n, uint8_t row_cover, uint8_t col_cover);
|
||||
#endif
|
||||
|
||||
/* Function Definition -------------------------------------------------------------------------- */
|
||||
int MUNKRES_get_assignment(int *assignment, float *cost, int m, int n) {
|
||||
int i = 0;
|
||||
int j = 0;
|
||||
int step;
|
||||
|
||||
#if PRINT_ALGO_RUN_TIME
|
||||
uint64 time_algo_1 = 0l;
|
||||
uint64 time_algo_2 = 0l;
|
||||
uint64 time_algo_3 = 0l;
|
||||
uint64 time_algo_4 = 0l;
|
||||
|
||||
#endif
|
||||
|
||||
if (m > 8 || n > 8) {
|
||||
return MUNKRES_EXCESSIVE_MATRIX;
|
||||
}
|
||||
|
||||
memcpy(g_cost, cost, m * n * sizeof(*g_cost));
|
||||
memset(g_mark, 0, m * n * sizeof(*g_mark));
|
||||
g_row_cover = 0;
|
||||
g_col_cover = 0;
|
||||
|
||||
step = 0;
|
||||
#if PRINT_ALGO_RUN_TIME
|
||||
time_algo_1 = uTimetick_Get();
|
||||
#endif
|
||||
while (7 != step) {
|
||||
#ifdef __MATLAB_DBG
|
||||
mexPrintf("step %d:\n", step);
|
||||
#endif
|
||||
switch (step) {
|
||||
case 0:
|
||||
case 1:
|
||||
step = step_one(g_cost, m, n);
|
||||
break;
|
||||
case 2:
|
||||
step = step_two(g_cost, g_mark, m, n, &g_col_cover);
|
||||
break;
|
||||
case 3:
|
||||
step = step_three(g_mark, m, n, &g_col_cover);
|
||||
break;
|
||||
case 4:
|
||||
step = step_four(g_cost, g_mark, m, n, &g_row_cover, &g_col_cover, &i, &j);
|
||||
#if PRINT_ALGO_RUN_TIME
|
||||
time_algo_3 = uTimetick_Get();
|
||||
time_algo_3 = (time_algo_3-time_algo_1);
|
||||
|
||||
#if defined(LOCATION_PLATFORM_QCOM_MODEM)
|
||||
Geek_PDR_LOGH_NMEA("step_4 run time ms %d",(int)time_algo_3,0,0,0);
|
||||
#endif
|
||||
#endif
|
||||
break;
|
||||
case 5:
|
||||
step = step_five(g_mark, m, n, &g_row_cover, &g_col_cover, i, j);
|
||||
#if PRINT_ALGO_RUN_TIME
|
||||
time_algo_4 = uTimetick_Get();
|
||||
time_algo_4 = (time_algo_4-time_algo_1);
|
||||
|
||||
#if defined(LOCATION_PLATFORM_QCOM_MODEM)
|
||||
Geek_PDR_LOGH_NMEA("step_5 run time ms %d",(int)time_algo_4,0,0,0);
|
||||
#endif
|
||||
#endif
|
||||
break;
|
||||
case 6:
|
||||
step = step_six(g_cost, m, n, &g_row_cover, &g_col_cover);
|
||||
break;
|
||||
default:
|
||||
step = 7;
|
||||
break;
|
||||
}
|
||||
#ifdef __MATLAB_DBG
|
||||
mexPrintf(" cost:\n", step);
|
||||
display_cost_matrix_and_cover(g_cost, m, n, g_row_cover, g_col_cover);
|
||||
mexPrintf(" mark:\n", step);
|
||||
display_mark_matrix_and_cover(g_mark, m, n, g_row_cover, g_col_cover);
|
||||
#endif
|
||||
}
|
||||
#if PRINT_ALGO_RUN_TIME
|
||||
time_algo_2 = uTimetick_Get();
|
||||
#endif
|
||||
#if PRINT_ALGO_RUN_TIME
|
||||
|
||||
time_algo_2 = (time_algo_2-time_algo_1);
|
||||
// time_algo_3 = (time_algo_3-time_algo_2);
|
||||
|
||||
#if defined(LOCATION_PLATFORM_QCOM_MODEM)
|
||||
Geek_PDR_LOGH_NMEA("munkres run time ms %d",(int)time_algo_2,0,0,0);
|
||||
#endif
|
||||
// Geek_PDR_LOGH_NMEA("pdr2 run time ms %d %d",(int)time_algo_5,time_algo_6,0,0);
|
||||
#endif
|
||||
|
||||
for (i = 0; i < m; ++i) {
|
||||
for (j = 0; j < n; ++j) {
|
||||
if (0 != g_mark[(i) + (j) * m]) {
|
||||
assignment[i] = j;
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
return MUNKRES_NO_ERROR;
|
||||
}
|
||||
|
||||
static int step_one(float *cost, int m, int n) {
|
||||
int i;
|
||||
int j;
|
||||
float min;
|
||||
for (i = 0; i < m; ++i) {
|
||||
min = COST(i, 0);
|
||||
for (j = 1; j < n; ++j) {
|
||||
if (min > COST(i, j)) {
|
||||
min = COST(i, j);
|
||||
}
|
||||
}
|
||||
for (j = 0; j < n; ++j) {
|
||||
COST(i, j) -= min;
|
||||
}
|
||||
}
|
||||
|
||||
return 2;
|
||||
}
|
||||
|
||||
static int step_two(float *cost, int *mark, int m, int n, uint8_t *col_cover) {
|
||||
int i;
|
||||
int j;
|
||||
|
||||
for (i = 0; i < m; ++i) {
|
||||
for (j = 0; j < n; ++j) {
|
||||
if (0 == COST(i, j) && 0 == TST_FLG(col, j)) {
|
||||
MARK(i, j) = 1;
|
||||
SET_FLG(col, j);
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
CLR_ALL(col);
|
||||
|
||||
return 3;
|
||||
}
|
||||
|
||||
static int step_three(int *mark, int m, int n, uint8_t *col_cover) {
|
||||
int i;
|
||||
int j;
|
||||
int count;
|
||||
|
||||
CLR_ALL(col);
|
||||
for (i = 0; i < m; ++i) {
|
||||
for (j = 0; j < n; ++j) {
|
||||
if (1 == MARK(i, j)) {
|
||||
SET_FLG(col, j);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
count = 0;
|
||||
for (j = 0; j < n; ++j) {
|
||||
count += (0 != TST_FLG(col, j) ? 1 : 0);
|
||||
}
|
||||
|
||||
return (count >= m || count >= n) ? 7 : 4;
|
||||
}
|
||||
|
||||
static void find_a_noncovered_zero(int *i, int *j, float *cost, int m, int n, uint8_t *row_cover, uint8_t *col_cover) {
|
||||
for (*i = 0; *i < m; ++*i) {
|
||||
if (0 != TST_FLG(row, *i)) {
|
||||
continue;
|
||||
}
|
||||
for (*j = 0; *j < n; ++*j) {
|
||||
if (0 == COST(*i, *j) && 0 == TST_FLG(col, *j)) {
|
||||
break;
|
||||
}
|
||||
}
|
||||
if (n != *j) {
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
static void find_a_star_zero_in_row(int *j, int i, int *mark, int m, int n) {
|
||||
for (*j = 0; *j < n; ++*j) {
|
||||
if (1 == MARK(i, *j)) {
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
static int step_four(float *cost, int *mark, int m, int n, uint8_t *row_cover, uint8_t *col_cover, int *path_start_i, int *path_start_j) {
|
||||
int i;
|
||||
int j;
|
||||
int jj;
|
||||
while (1) {
|
||||
find_a_noncovered_zero(&i, &j, cost, m, n, row_cover, col_cover);
|
||||
if (m == i) {
|
||||
return 6;
|
||||
}
|
||||
else {
|
||||
MARK(i, j) = 2;
|
||||
find_a_star_zero_in_row(&jj, i, mark, m, n);
|
||||
if (m != jj) {
|
||||
SET_FLG(row, i);
|
||||
CLR_FLG(col, jj);
|
||||
j = jj;
|
||||
}
|
||||
else {
|
||||
*path_start_i = i;
|
||||
*path_start_j = j;
|
||||
return 5;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
static void find_a_star_zero_in_col(int *i, int j, int *mark, int m) {
|
||||
for (*i = 0; *i < m; ++*i) {
|
||||
if (1 == MARK(*i, j)) {
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
static void find_a_prime_zero_in_row(int *j, int i, int *mark, int m, int n) {
|
||||
for (*j = 0; *j < n; ++*j) {
|
||||
if (2 == MARK(i, *j)) {
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
static int step_five(int *mark, int m, int n, uint8_t *row_cover, uint8_t *col_cover, int path_start_i, int path_start_j) {
|
||||
static int path[64 * 2];
|
||||
int i;
|
||||
int j;
|
||||
int k;
|
||||
|
||||
k = 0;
|
||||
PATH(k, 0) = path_start_i;
|
||||
PATH(k, 1) = path_start_j;
|
||||
while (1) {
|
||||
find_a_star_zero_in_col(&i, PATH(k, 1), mark, m);
|
||||
if (m == i) {
|
||||
goto STEP_FIVE_EXIT;
|
||||
}
|
||||
else {
|
||||
PATH(k + 1, 0) = i;
|
||||
PATH(k + 1, 1) = PATH(k, 1);
|
||||
++k;
|
||||
find_a_prime_zero_in_row(&j, PATH(k, 0), mark, m, n);
|
||||
PATH(k + 1, 0) = PATH(k, 0);
|
||||
PATH(k + 1, 1) = j;
|
||||
++k;
|
||||
}
|
||||
}
|
||||
|
||||
STEP_FIVE_EXIT:
|
||||
/* augment path */
|
||||
for(; k >= 0; --k) {
|
||||
MARK(PATH(k, 0), PATH(k, 1)) = (1 == MARK(PATH(k, 0), PATH(k, 1)) ? 0 : 1);
|
||||
}
|
||||
/* clear covers */
|
||||
CLR_ALL(row);
|
||||
CLR_ALL(col);
|
||||
/* erease primes */
|
||||
for (i = 0; i < m; ++i) {
|
||||
for (j = 0; j < n; ++j) {
|
||||
if (2 == MARK(i, j)) {
|
||||
MARK(i, j) = 0;
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
return 3;
|
||||
}
|
||||
|
||||
static int step_six(float *cost, int m, int n, uint8_t *row_cover, uint8_t *col_cover) {
|
||||
float min;
|
||||
int i;
|
||||
int j;
|
||||
|
||||
min = 1e6;
|
||||
for (i = 0; i < m; ++i) {
|
||||
for (j = 0; j < n; ++j) {
|
||||
if (0 == TST_FLG(row, i) && 0 == TST_FLG(col, j)) {
|
||||
if (min > COST(i, j)) {
|
||||
min = COST(i, j);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
for (i = 0; i < m; ++i) {
|
||||
for (j = 0; j < n; ++j) {
|
||||
if (0 != TST_FLG(row, i)) {
|
||||
COST(i, j) += min;
|
||||
}
|
||||
if (0 == TST_FLG(col, j)) {
|
||||
COST(i, j) -= min;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
return 4;
|
||||
}
|
||||
|
||||
|
||||
|
|
@ -1,422 +0,0 @@
|
|||
/******************** (C) COPYRIGHT 2020 Geek************************************
|
||||
* File Name : pdr_ahrs.c
|
||||
* Department : Sensor Algorithm Team
|
||||
* Current Version : V1.2
|
||||
* Author : logzhan &
|
||||
*
|
||||
*
|
||||
* Date of Issued : 2020.7.3
|
||||
* Comments : PDR AHRS 姿态解算相关
|
||||
********************************************************************************/
|
||||
/* Header File Including -----------------------------------------------------*/
|
||||
#include <math.h>
|
||||
#include <string.h>
|
||||
#include "pdr_base.h"
|
||||
#include "pdr_ahrs.h"
|
||||
#include "pdr_detector.h"
|
||||
#include "Filter.h"
|
||||
#include "buffer.h"
|
||||
#include "pdr_kalman.h"
|
||||
#include "pdr_util.h"
|
||||
|
||||
/* Macro Definition ----------------------------------------------------------*/
|
||||
#define AHRS_BUF_LEN 256
|
||||
#define FLT_THRES 0.000001f // 浮点数最小阈值
|
||||
//计算模值
|
||||
#define GET_MOL(v) sqrt((v)[0] * (v)[0] + (v)[1] * (v)[1] + (v)[2] * (v)[2])
|
||||
//获取数组长度
|
||||
#define ARR_LEN(arr) (sizeof(arr) / sizeof(*arr))
|
||||
#define SET_BIT(msk) (g_ahrs.status |= AHRS_STATUS_##msk)
|
||||
#define CLR_BIT(msk) (g_ahrs.status &= ~AHRS_STATUS_##msk)
|
||||
#define TST_BIT(msk) (g_ahrs.status & AHRS_STATUS_##msk)
|
||||
#define INC_PTR(buf, ptr) (((ptr) + 1) % ((buf).length + 1))
|
||||
|
||||
#define GRV_BUF_NME(i) "grav_buf_##i"
|
||||
#define GYR_BUF_NME(i) "gyro_buf_##i"
|
||||
#define MAG_BUF_NME(i) "magn_buf_##i"
|
||||
|
||||
/* Global Variable Definition ------------------------------------------------*/
|
||||
/* Macro Definition ----------------------------------------------------------*/
|
||||
|
||||
/* Global Variable Definition ------------------------------------------------*/
|
||||
static IMU g_imu;
|
||||
AHRS g_ahrs;
|
||||
static uint64_t g_ticker;
|
||||
static BUFFER_LONG g_erro_buf;
|
||||
static BUFFER_LONG g_grav_buf[3];
|
||||
static BUFFER_LONG g_gyro_buf[3];
|
||||
static BUFFER_LONG g_magn_buf[3];
|
||||
|
||||
static float dt = 1.0f / AHRS_SAMPLE_FREQ; // sample interval in second
|
||||
static float Kp = 0; // 2 * proportional gain (Kp)
|
||||
static float q0 = 1.0f, q1 = 0.0f, q2 = 0.0f, q3 = 0.0f; // 初始四元数
|
||||
|
||||
static const float g_x_axis[] = {1, 0, 0};
|
||||
static const float g_y_axis[] = {0, 1, 0};
|
||||
static const float g_z_axis[] = {0, 0, 1};
|
||||
|
||||
/**----------------------------------------------------------------------
|
||||
* Function : mahonyUpdateAHRS
|
||||
* Description : mahony姿态更新算法
|
||||
* Date : 2020/8/3 logzhan
|
||||
* 2020/02/18 logzhan :
|
||||
* 增加了AHRS稳定性标志位，如果AHRS稳定后不再计算error, 降低运算
|
||||
量。能提升PDR仿真10%的运行速度
|
||||
*---------------------------------------------------------------------**/
|
||||
static void mahonyUpdateAHRS(float gx, float gy, float gz, float ax, float ay, float az,
|
||||
float mx, float my, float mz) {
|
||||
// 归一化磁力计和加速度计
|
||||
pdr_v3Norm(&ax, &ay, &az);
|
||||
|
||||
float wx = 0.0f, wy = 0.0f, wz = 0.0f;
|
||||
|
||||
|
||||
//if (!((mx == 0.0f) && (my == 0.0f) && (mz == 0.0f))) {
|
||||
|
||||
// pdr_v3Norm(&mx, &my, &mz);
|
||||
|
||||
// // 把磁场从载体系转换到地理坐标系 h = R^-1 * (mx,my,mz)
|
||||
// float hx = 2.0f * (mx * (0.5f - q2 * q2 - q3 * q3) + my * (q1 * q2 - q0 * q3) + mz * (q1 * q3 + q0 * q2));
|
||||
// float hy = 2.0f * (mx * (q1 * q2 + q0 * q3) + my * (0.5f - q1 * q1 - q3 * q3) + mz * (q2 * q3 - q0 * q1));
|
||||
// float hz = 2.0f * (mx * (q1 * q3 - q0 * q2) + my * (q2 * q3 + q0 * q1) + mz * (0.5f - q1 * q1 - q2 * q2));
|
||||
|
||||
// // 理论上bx = 0, by 指向北向，因为手机IMU数据使用的是东北天(x,y,z)坐标系
|
||||
// float by = (float)sqrt(hx * hx + hy * hy);
|
||||
// float bz = hz;
|
||||
|
||||
// wx = by * (q0 * q3 + q1 * q2) + bz * (q1 * q3 - q0 * q2);
|
||||
// wy = by * (0.5f - q1 * q1 - q3 * q3) + bz * (q0 * q1 + q2 * q3);
|
||||
// wz = by * (q2 * q3 - q0 * q1) + bz * (0.5f - q1 * q1 - q2 * q2);
|
||||
//}
|
||||
|
||||
// 把重力转换到地理坐标系 v = R * (0,0,1)
|
||||
float vx = q1*q3 - q0*q2;
|
||||
float vy = q0*q1 + q2*q3;
|
||||
float vz = q0*q0 - 0.5f + q3*q3;
|
||||
|
||||
// 计算矢量叉乘误差
|
||||
float ex = ay*vz - az*vy + my*wz - mz*wy;
|
||||
float ey = az*vx - ax*vz + mz*wx - mx*wz;
|
||||
float ez = ax*vy - ay*vx + mx*wy - my*wx;
|
||||
|
||||
// Apply proportional feedback
|
||||
gx += Kp * ex;
|
||||
gy += Kp * ey;
|
||||
gz += Kp * ez;
|
||||
|
||||
// 这两个函数占了大量的时间
|
||||
if (g_ahrs.stable == PDR_FALSE) {
|
||||
buffer_push((BUFFER*)&g_erro_buf, (float)(2 * sqrt(ex * ex + ey * ey + ez * ez)));
|
||||
Buffer_mean((BUFFER*)&g_erro_buf, &g_ahrs.error);
|
||||
}
|
||||
|
||||
// 龙格库塔法更新四元数
|
||||
float qa = q0; float qb = q1; float qc = q2;
|
||||
q0 += (-qb * gx - qc * gy - q3 * gz) * 0.5f * (dt);
|
||||
q1 += ( qa * gx + qc * gz - q3 * gy) * 0.5f * (dt);
|
||||
q2 += ( qa * gy - qb * gz + q3 * gx) * 0.5f * (dt);
|
||||
q3 += ( qa * gz + qb * gy - qc * gx) * 0.5f * (dt);
|
||||
|
||||
// 四元数归一化
|
||||
QuatNorm(&q0, &q1, &q2, &q3);
|
||||
|
||||
// 四元数转欧拉角
|
||||
g_ahrs.pitch = (float)asin(-2.0f * (q3*q1 - q0*q2)) * (180.0f / 3.141592f);
|
||||
g_ahrs.yaw = (float)atan2(q2*q1 + q0*q3, 0.5f - q2*q2 - q3*q3) * (180.0f / 3.141592f);
|
||||
g_ahrs.roll = (float)atan2(q2*q3 + q0*q1, 0.5f - q2*q2 - q1*q1) * (180.0f / 3.141592f);
|
||||
}
|
||||
|
||||
/**---------------------------------------------------------------------
|
||||
* Function : AHRS_sensorFrame2EarthFrame
|
||||
* Description : 转换加速度计值到地球坐标系
|
||||
* Date : 2020/7/20 logzhan
|
||||
*---------------------------------------------------------------------**/
|
||||
void AHRS_sensorFrame2EarthFrame(float e[], float s[]) {
|
||||
float quaVec[4];
|
||||
float quaTmp[4];
|
||||
float quaCnj[4];
|
||||
|
||||
quaVec[0] = 0;
|
||||
quaVec[1] = s[0];
|
||||
quaVec[2] = s[1];
|
||||
quaVec[3] = s[2];
|
||||
quaternProd(quaTmp, g_ahrs.qnb, quaVec);
|
||||
QuaternConj(quaCnj, g_ahrs.qnb);
|
||||
quaternProd(quaVec, quaTmp, quaCnj);
|
||||
e[0] = quaVec[1];
|
||||
e[1] = quaVec[2];
|
||||
e[2] = quaVec[3];
|
||||
}
|
||||
|
||||
void pdr_initAhrs(void) {
|
||||
int i;
|
||||
g_ahrs.stable = PDR_FALSE;
|
||||
Buffer_initialize((BUFFER *)&g_erro_buf, "erro_buf", BUFFER_TYPE_QUEUE, AHRS_BUF_LEN);
|
||||
for (i = 0; i < 3; ++i) {
|
||||
Buffer_initialize((BUFFER *)&g_grav_buf[i], GRV_BUF_NME(i), BUFFER_TYPE_QUEUE, AHRS_BUF_LEN);
|
||||
Buffer_initialize((BUFFER *)&g_gyro_buf[i], GYR_BUF_NME(i), BUFFER_TYPE_QUEUE, AHRS_BUF_LEN);
|
||||
Buffer_initialize((BUFFER *)&g_magn_buf[i], MAG_BUF_NME(i), BUFFER_TYPE_QUEUE, AHRS_BUF_LEN);
|
||||
}
|
||||
pdr_ahrsReset();
|
||||
}
|
||||
|
||||
void pdr_ahrsReset(void) {
|
||||
|
||||
Buffer_clear((BUFFER *)&g_erro_buf);
|
||||
for (uchar i = 0; i < 3; ++i) {
|
||||
Buffer_clear((BUFFER *)&g_grav_buf[i]);
|
||||
Buffer_clear((BUFFER *)&g_gyro_buf[i]);
|
||||
Buffer_clear((BUFFER *)&g_magn_buf[i]);
|
||||
}
|
||||
g_ticker = 0;
|
||||
q0 = 1.0f;
|
||||
q1 = 0.0f;
|
||||
q2 = 0.0f;
|
||||
q3 = 0.0f;
|
||||
memset(&g_ahrs, 0, sizeof(g_ahrs));
|
||||
g_ahrs.status = (uint8_t)AHRS_STATUS_RESET_PHASE_0;
|
||||
g_ahrs.error = -1;
|
||||
g_ahrs.qnb[0] = 1.0f;
|
||||
g_ahrs.qnb[1] = 0.0f;
|
||||
g_ahrs.qnb[2] = 0.0f;
|
||||
g_ahrs.qnb[3] = 0.0f;
|
||||
g_ahrs.gravity[0] = 0.0f;
|
||||
g_ahrs.gravity[1] = 0.0f;
|
||||
g_ahrs.gravity[2] = 9.8f;
|
||||
g_ahrs.x_axis[0] = 1.0f;
|
||||
g_ahrs.x_axis[1] = 0.0f;
|
||||
g_ahrs.x_axis[2] = 0.0f;
|
||||
g_ahrs.y_axis[0] = 0.0f;
|
||||
g_ahrs.y_axis[1] = 1.0f;
|
||||
g_ahrs.y_axis[2] = 0.0f;
|
||||
g_ahrs.z_axis[0] = 0.0f;
|
||||
g_ahrs.z_axis[1] = 0.0f;
|
||||
g_ahrs.z_axis[2] = 1.0f;
|
||||
}
|
||||
|
||||
/**---------------------------------------------------------------------
|
||||
* Function : estimate_sensor_vector
|
||||
* Description : 利用当前四元数q与陀螺仪，计算在当前姿态下grav
|
||||
* input : float eVec[], float gyro[]
|
||||
* output : float sVec[]
|
||||
* Date : 2020/7/20 logzhan
|
||||
*---------------------------------------------------------------------**/
|
||||
static void estimate_sensor_vector(float sVec[], float eVec[], float gyro[]) {
|
||||
float q[4];
|
||||
float gx, gy, gz;
|
||||
float qa, qb, qc;
|
||||
float recipNorm;
|
||||
float quaVec[4];
|
||||
float quaTmp[4];
|
||||
float quaCnj[4];
|
||||
|
||||
/* estimate */
|
||||
gx = gyro[0] * (0.5f * (dt));
|
||||
gy = gyro[1] * (0.5f * (dt));
|
||||
gz = gyro[2] * (0.5f * (dt));
|
||||
qa = q0;
|
||||
qb = q1;
|
||||
qc = q2;
|
||||
q[0] = q0 + (-qb * gx - qc * gy - q3 * gz);
|
||||
q[1] = q1 + (qa * gx + qc * gz - q3 * gy);
|
||||
q[2] = q2 + (qa * gy - qb * gz + q3 * gx);
|
||||
q[3] = q3 + (qa * gz + qb * gy - qc * gx);
|
||||
recipNorm = InvSqrt(q[0] * q[0] + q[1] * q[1] + q[2] * q[2] + q[3] * q[3]);
|
||||
q[0] *= recipNorm;
|
||||
q[1] *= recipNorm;
|
||||
q[2] *= recipNorm;
|
||||
q[3] *= recipNorm;
|
||||
|
||||
/* project */
|
||||
QuaternConj(q, q);
|
||||
quaVec[0] = 0;
|
||||
quaVec[1] = eVec[0];
|
||||
quaVec[2] = eVec[1];
|
||||
quaVec[3] = eVec[2];
|
||||
quaternProd(quaTmp, q, quaVec);
|
||||
QuaternConj(quaCnj, q);
|
||||
quaternProd(quaVec, quaTmp, quaCnj);
|
||||
sVec[0] = quaVec[1];
|
||||
sVec[1] = quaVec[2];
|
||||
sVec[2] = quaVec[3];
|
||||
}
|
||||
/**----------------------------------------------------------------------
|
||||
* Function : pdr_imuUpdateAhrs
|
||||
* Description : 1、AHRS融合解算
|
||||
* 2、Error分析
|
||||
* 3、PCA方向计算
|
||||
* Date : 2020/6/30 logzhan
|
||||
*---------------------------------------------------------------------**/
|
||||
int pdr_imuUpdateAhrs(imu* imu) {
|
||||
float gyro[3], acce[3], magn[3], grav[3], mean[2];
|
||||
int index;
|
||||
int count;
|
||||
int i;
|
||||
|
||||
for (uchar i = 0; i < 3; ++i) {
|
||||
gyro[i] = imu->gyr.s[i];
|
||||
acce[i] = imu->acc.s[i];
|
||||
magn[i] = imu->mag.s[i];
|
||||
}
|
||||
|
||||
//g_ahrs.status(初始状态0x80) 按位与 AHRS_STATUS_RESET_PHASE_0(0x80)
|
||||
if (g_ahrs.status & AHRS_STATUS_RESET_PHASE_0) {
|
||||
|
||||
// ARHS BUF 中存储了256个sensor的缓存
|
||||
if (g_ticker <= AHRS_BUF_LEN) {
|
||||
for (i = 0; i < 3; ++i) {
|
||||
buffer_push((BUFFER *)&g_grav_buf[i], acce[i]);
|
||||
buffer_push((BUFFER *)&g_gyro_buf[i], gyro[i]);
|
||||
buffer_push((BUFFER *)&g_magn_buf[i], magn[i]);
|
||||
}
|
||||
}
|
||||
// 只有缓存满的时候才计算
|
||||
if (g_ticker >= AHRS_BUF_LEN) {
|
||||
//int index;
|
||||
index = (g_magn_buf[0]._top + AHRS_BUF_LEN + 1 - AHRS_BUF_LEN / 2) % (g_magn_buf[0].length + 1);
|
||||
for (i = 0; i < 3; ++i) {
|
||||
Buffer_mean((BUFFER *)&g_grav_buf[i], &grav[i]);
|
||||
magn[i] = g_magn_buf[i].data[index];
|
||||
g_grav_buf[i]._top = g_grav_buf[i]._bottom;
|
||||
}
|
||||
|
||||
Kp = (float)(2 * 2);
|
||||
/* 这里循环运行1000次,收敛，趋于正确姿态 */
|
||||
for (i = 0; i < 1000; ++i) {
|
||||
mahonyUpdateAHRS(0, 0, 0, grav[0], grav[1], grav[2], magn[0], magn[1], magn[2]);
|
||||
}
|
||||
|
||||
AHRS_sensorFrame2EarthFrame(g_ahrs.gravity, grav);
|
||||
|
||||
Kp = (float)(2 * 2);
|
||||
for (index = INC_PTR(g_gyro_buf[0], index); index != g_gyro_buf[0]._top; index = INC_PTR(g_gyro_buf[0], index)) {
|
||||
for (i = 0; i < 3; ++i) {
|
||||
buffer_push((BUFFER *)&g_grav_buf[i], g_ahrs.gravity[i]);
|
||||
Buffer_mean((BUFFER *)&g_grav_buf[i], &grav[i]);
|
||||
gyro[i] = g_gyro_buf[i].data[index];
|
||||
acce[i] = g_grav_buf[i].data[index];
|
||||
magn[i] = g_magn_buf[i].data[index];
|
||||
}
|
||||
estimate_sensor_vector(grav, grav, gyro);
|
||||
|
||||
// AHRS 更新
|
||||
mahonyUpdateAHRS(gyro[0], gyro[1], gyro[2], grav[0], grav[1], grav[2], magn[0],
|
||||
magn[1], magn[2]);
|
||||
|
||||
AHRS_sensorFrame2EarthFrame(g_ahrs.gravity, acce);
|
||||
}
|
||||
|
||||
g_ahrs.qnb[0] = q0;
|
||||
g_ahrs.qnb[1] = q1;
|
||||
g_ahrs.qnb[2] = q2;
|
||||
g_ahrs.qnb[3] = q3;
|
||||
|
||||
CLR_BIT(RESET_PHASE_0); /*#define CLR_BIT(msk) (g_ahrs.status &= ~AHRS_STATUS_##msk)；清零g_ahrs.status*/
|
||||
SET_BIT(RESET_PHASE_1); /*#define SET_BIT(msk) (g_ahrs.status |= AHRS_STATUS_##msk)；赋值g_ahrs.status，与phase1相同*/
|
||||
}
|
||||
++g_ticker;
|
||||
//return;
|
||||
//g_ahrs.status(0x80) 按位与 AHRS_STATUS_RESET_PHASE_1(0x40)
|
||||
}else if (g_ahrs.status & AHRS_STATUS_RESET_PHASE_1) {
|
||||
//PDR_LOGD("阶段1");
|
||||
// 应该是判断是否进入PCA的条件
|
||||
if (g_ahrs.error < 0.3) {
|
||||
|
||||
Buffer_count((BUFFER *)&g_erro_buf, &count);
|
||||
memset((void *)mean, 0, sizeof(mean));
|
||||
for (index = g_erro_buf._bottom, i = 0; index != g_erro_buf._top; index = INC_PTR(g_erro_buf, index), ++i) {
|
||||
int c = count >> 1;
|
||||
if (i < c ) {
|
||||
mean[0] += g_erro_buf.data[index];
|
||||
}
|
||||
else {
|
||||
mean[1] += g_erro_buf.data[index];
|
||||
}
|
||||
}
|
||||
int a = count >> 1;
|
||||
int b = count - (count >> 1);
|
||||
mean[0] /= a;
|
||||
mean[1] /= b;
|
||||
|
||||
if (fabs(mean[0] - mean[1]) < 0.025) {
|
||||
Kp = (float)(2 * AHRS_KP);
|
||||
|
||||
// 这个标志位决定是否进入PCA方向估计
|
||||
CLR_BIT(RESET_PHASE_1);
|
||||
SET_BIT(STABLE);
|
||||
g_ahrs.stable = PDR_TRUE;
|
||||
}
|
||||
}
|
||||
|
||||
for (i = 0; i < 3; ++i) {
|
||||
buffer_push((BUFFER *)&g_grav_buf[i], g_ahrs.gravity[i]);
|
||||
Buffer_mean((BUFFER *)&g_grav_buf[i], &grav[i]);
|
||||
}
|
||||
estimate_sensor_vector(grav, grav, gyro);
|
||||
|
||||
mahonyUpdateAHRS(gyro[0], gyro[1], gyro[2],
|
||||
grav[0], grav[1], grav[2],
|
||||
magn[0], magn[1], magn[2]);
|
||||
|
||||
g_ahrs.qnb[0] = q0;
|
||||
g_ahrs.qnb[1] = q1;
|
||||
g_ahrs.qnb[2] = q2;
|
||||
g_ahrs.qnb[3] = q3;
|
||||
|
||||
AHRS_sensorFrame2EarthFrame(g_ahrs.gravity, acce);
|
||||
AHRS_sensorFrame2EarthFrame(g_ahrs.x_axis, (float*)g_x_axis);
|
||||
AHRS_sensorFrame2EarthFrame(g_ahrs.y_axis, (float*)g_y_axis);
|
||||
AHRS_sensorFrame2EarthFrame(g_ahrs.z_axis, (float*)g_z_axis);
|
||||
|
||||
pdr_computePCA(&g_ahrs);
|
||||
|
||||
++g_ticker;
|
||||
}
|
||||
else {
|
||||
|
||||
// Buffer_mean的计算时间太长了，10ms更新来算，计算256个数的平均
|
||||
// 计算量实在太大
|
||||
//for (i = 0; i < 3; ++i) {
|
||||
// Buffer_push((BUFFER *)&g_grav_buf[i], g_ahrs.gravity[i]);
|
||||
// Buffer_mean((BUFFER *)&g_grav_buf[i], &grav[i]);
|
||||
//}
|
||||
//estimate_sensor_vector(grav, grav, gyro);
|
||||
|
||||
// logzhan 21/02/06 : 感觉是acc输入精度会高一些
|
||||
mahonyUpdateAHRS(gyro[0], gyro[1], gyro[2],
|
||||
acce[0], acce[1], acce[2],
|
||||
magn[0], magn[1], magn[2]);
|
||||
|
||||
g_ahrs.qnb[0] = q0;
|
||||
g_ahrs.qnb[1] = q1;
|
||||
g_ahrs.qnb[2] = q2;
|
||||
g_ahrs.qnb[3] = q3;
|
||||
|
||||
// 把载体系的加速度转换为地理坐标系,
|
||||
AHRS_sensorFrame2EarthFrame(g_ahrs.gravity, acce);
|
||||
// 这部分代码都是可以简化以增加运算速度的，暂时不清楚作用
|
||||
AHRS_sensorFrame2EarthFrame(g_ahrs.x_axis, (float*)g_x_axis);
|
||||
AHRS_sensorFrame2EarthFrame(g_ahrs.y_axis, (float*)g_y_axis);
|
||||
AHRS_sensorFrame2EarthFrame(g_ahrs.z_axis, (float*)g_z_axis);
|
||||
|
||||
++g_ticker;
|
||||
return PDR_TRUE;
|
||||
}
|
||||
return PDR_FALSE;
|
||||
}
|
||||
|
||||
/**---------------------------------------------------------------------
|
||||
* Function : fv3Norm
|
||||
* Description : 三维浮点数向量归一化
|
||||
* Date : 2021/01/26 logzhan
|
||||
*---------------------------------------------------------------------**/
|
||||
void fv3Norm(float* vx, float* vy, float* vz)
|
||||
{
|
||||
float norm = sqrtf(((*vx) * (*vx) + (*vy) * (*vy) +
|
||||
(*vz) * (*vz)));
|
||||
// 防止出现模为0的情况
|
||||
if (norm > FLT_THRES) {
|
||||
norm = 1 / norm;
|
||||
(*vx) *= norm; (*vy) *= norm; (*vz) *= norm;
|
||||
}
|
||||
}
|
||||
|
|
@ -1,256 +0,0 @@
|
|||
/******************** (C) COPYRIGHT 2020 Geek************************************
|
||||
* File Name : pdr_api.c
|
||||
* Department : Sensor Algorithm Team
|
||||
* Current Version : V2.0
|
||||
* Author :
|
||||
*
|
||||
* Date of Issued : 2020.7.3
|
||||
* Comments : PDR导航算法对外部接口
|
||||
********************************************************************************/
|
||||
/* Header File Including -----------------------------------------------------*/
|
||||
#include <stdio.h>
|
||||
#include <math.h>
|
||||
#include <string.h>
|
||||
#include "pdr_api.h"
|
||||
#include "pdr_base.h"
|
||||
#include "steps.h"
|
||||
#include "pdr_location.h"
|
||||
#include "parseData.h"
|
||||
#include "scene_recognition.h"
|
||||
#include "pdr_trackSmooth.h"
|
||||
#include "LapProcess.h"
|
||||
|
||||
/* Global Variable Definition ------------------------------------------------*/
|
||||
const char* pdrVersion = "1.0";
|
||||
size_t slideWindowSize = 5;
|
||||
sensor_agm SensorDataHist[IMU_LAST_COUNT] = {{0}};
|
||||
LatLng lla_last = { 0 };
|
||||
lct_nmea ln;
|
||||
imu imuData;
|
||||
|
||||
/* Extern Variable Definition ------------------------------------------------*/
|
||||
extern KfPara g_kfPara;
|
||||
extern AHRS g_ahrs;
|
||||
extern gnss pgnss;
|
||||
|
||||
/* Function Declaration ------------------------------------------------------*/
|
||||
|
||||
/**---------------------------------------------------------------------
|
||||
* Function : pdr_initRmc
|
||||
* Description : 初始化RMC协议
|
||||
* Date : 2020/7/3 logzhan
|
||||
*---------------------------------------------------------------------**/
|
||||
void RMC_Init(lct_nmea_rmc * rmc)
|
||||
{
|
||||
rmc->type = SENSOR_MIN;
|
||||
rmc->rmc_utc = ITEM_INVALID;
|
||||
rmc->status = POSITIONING_N;
|
||||
rmc->latitude = ITEM_INVALID;
|
||||
rmc->rmc_latitude_ns = LATITUDE_N;
|
||||
rmc->longitudinal = ITEM_INVALID;
|
||||
rmc->longitudinal_ew = LATITUDE_N;
|
||||
rmc->speed = ITEM_INVALID;
|
||||
rmc->cog = ITEM_INVALID;
|
||||
rmc->utc_ddmmyy = ITEM_INVALID;
|
||||
rmc->magnetic = ITEM_INVALID;
|
||||
rmc->mag_h = MAG_E;
|
||||
rmc->mode = MODE_A;
|
||||
rmc->time = ITEM_INVALID;
|
||||
rmc->update = UN_UPDATE;
|
||||
|
||||
memset(rmc->rmc_check_sum, 0, sizeof(rmc->rmc_check_sum));
|
||||
}
|
||||
|
||||
/**---------------------------------------------------------------------
|
||||
* Function : pdr_initNmeaFlg
|
||||
* Description : 初始化GPS的NMEA协议的相关标志位
|
||||
* Date : 2020/7/3 logzhan
|
||||
*---------------------------------------------------------------------**/
|
||||
void NmeaFlag_Init(lct_nmea * ln)
|
||||
{
|
||||
ln->gga.update = 0;
|
||||
for (char i = 0; i < 3; i++) {
|
||||
ln->gsa[i].update = UN_UPDATE;
|
||||
ln->gsv[i].update = UN_UPDATE;
|
||||
}
|
||||
|
||||
ln->minTime = 0;
|
||||
ln->maxTime = 0;
|
||||
ln->update = UN_UPDATE;
|
||||
|
||||
RMC_Init(&ln->rmc[0]);
|
||||
RMC_Init(&ln->rmc[1]);
|
||||
|
||||
ln->accuracy.time = ITEM_INVALID;
|
||||
ln->accuracy.update = UN_UPDATE;
|
||||
ln->accuracy.status = ITEM_INVALID;
|
||||
ln->accuracy.err = ITEM_INVALID;
|
||||
}
|
||||
|
||||
|
||||
/**---------------------------------------------------------------------
|
||||
* Function : clearNmeaFlg
|
||||
* Description : 清空Nmea标志位
|
||||
* Date : 2020/7/4 logzhan
|
||||
*---------------------------------------------------------------------**/
|
||||
void clearNmeaFlg(lct_nmea * ln){
|
||||
NmeaFlag_Init(ln);
|
||||
}
|
||||
|
||||
/**---------------------------------------------------------------------
|
||||
* Function : Algorithm_Init
|
||||
* Description : PDR算法主初始化流程，包括PDR导航初始化、计步器状态初始化以及轨
|
||||
* 迹平滑窗口的初始化
|
||||
* Date : 2020/7/3 logzhan
|
||||
*---------------------------------------------------------------------**/
|
||||
void Algorithm_Init(void) {
|
||||
// 导航相关初始化
|
||||
NavSys_Init();
|
||||
// 计步器初始化
|
||||
Pedometer_Init();
|
||||
// 设置轨迹平滑滑动窗口大小
|
||||
TrackSmoothSetWinSize(slideWindowSize);
|
||||
// 跑圈优化初始化
|
||||
lapInit();
|
||||
// 初始化sensor历史缓存
|
||||
memset(SensorDataHist, 0, sizeof(SensorDataHist));
|
||||
memset(&ln, 0, sizeof(ln));
|
||||
memset(&imuData, 0, sizeof(imuData));
|
||||
}
|
||||
|
||||
/**---------------------------------------------------------------------
|
||||
* Function : pdr_locationMainLoopStr
|
||||
* Description : PDR定位主循环，主要处理来自上层输入的传感器数据并解算位置
|
||||
* 迹平滑窗口的初始化
|
||||
* Date : 2020/07/03 logzhan
|
||||
* 2020/02/02 logzhan : 增加宏控制是否开启输出平滑
|
||||
*---------------------------------------------------------------------**/
|
||||
int pdr_locationMainLoopStr(char* line, lct_fs* result) {
|
||||
|
||||
pdr_parseLineStr(line, &imuData, &ln);
|
||||
return pdr_locationMainLoop(&imuData, &ln, result, NULL);
|
||||
}
|
||||
|
||||
|
||||
/**---------------------------------------------------------------------
|
||||
* Function : pdr_locationMainLoop
|
||||
* Description : PDR定位主循环，主要处理来自上层输入的传感器数据并解算位置
|
||||
* 迹平滑窗口的初始化
|
||||
* Date : 2020/07/03 logzhan
|
||||
* 2020/02/02 logzhan : 增加宏控制是否开启输出平滑
|
||||
*---------------------------------------------------------------------**/
|
||||
int pdr_locationMainLoop(imu *ss_data, lct_nmea *nmea_data, lct_fs *result, FILE *fp_gps) {
|
||||
int type = 0;
|
||||
|
||||
if (ss_data->gyr.update) {
|
||||
// 如果陀螺仪更新则采用惯性传感器计算
|
||||
pdr_insLocation(ss_data,&g_kfPara);
|
||||
ss_data->gyr.update = UN_UPDATE;
|
||||
}
|
||||
// 如果GPS不更新，返回
|
||||
if (!nmea_data->update)return TYPE_FIX_NONE;
|
||||
// 保存GPS相关信息
|
||||
pdr_saveGnssInfo(nmea_data, result, fp_gps);
|
||||
|
||||
// 有GPS则采用GPS融合定位
|
||||
int flag = pdr_gnssInsLocation(nmea_data, &g_kfPara, result);
|
||||
|
||||
if (flag != TYPE_FIX_NONE) {
|
||||
// 如果是开车这种，直接输出GPS，不进行平滑处理
|
||||
if (flag == TYPE_FIX_GPS) {
|
||||
result->latitude = R2D(result->latitude);
|
||||
result->longitudinal = R2D(result->longitudinal);
|
||||
|
||||
lapProcess(pgnss, result);
|
||||
} else {
|
||||
LatLng lla = { 0 };
|
||||
lla.lat = R2D(result->latitude);
|
||||
lla.lon = R2D(result->longitudinal);
|
||||
|
||||
if (result->last_lat != 0.0 && result->last_lon != 0.0 &&
|
||||
calDistance(lla, lla_last) > 10) {
|
||||
TrackSmoothSetWinSize(slideWindowSize);
|
||||
}
|
||||
result->latitude = R2D(result->latitude);
|
||||
result->longitudinal = R2D(result->longitudinal);
|
||||
lapProcess(pgnss, result);
|
||||
result->latitude = (result->latitude) * RADIAN_PER_DEG;
|
||||
result->longitudinal = (result->longitudinal) * RADIAN_PER_DEG;
|
||||
|
||||
#if PDR_OUTPUT_SMOOTH
|
||||
double outLat = 0, outLon = 0;
|
||||
// 输出轨迹平滑
|
||||
pdr_trackSmooth(R2D(result->latitude), R2D(result->longitudinal) ,
|
||||
&outLat, &outLon);
|
||||
|
||||
result->latitude = outLat;
|
||||
result->longitudinal = outLon;
|
||||
#else
|
||||
result->latitude = R2D(result->latitude);
|
||||
result->longitudinal = R2D(result->longitudinal);
|
||||
#endif
|
||||
result->last_lat = result->latitude;
|
||||
result->last_lon = result->longitudinal;
|
||||
|
||||
lla_last.lat = lla.lat;
|
||||
lla_last.lon = lla.lon;
|
||||
}
|
||||
type = 1;
|
||||
}else if (result->last_lat != 0.0 && result->last_lon != 0.0) {
|
||||
result->latitude = result->last_lat;
|
||||
result->longitudinal = result->last_lon;
|
||||
type = 1;
|
||||
lapProcess(pgnss, result);
|
||||
}
|
||||
clearNmeaFlg(nmea_data);
|
||||
return type;
|
||||
}
|
||||
|
||||
/**---------------------------------------------------------------------
|
||||
* Function : pdr_saveGnssInfo
|
||||
* Description : 保存GPS相关信息
|
||||
* Date : 2020/7/3 logzhan
|
||||
*---------------------------------------------------------------------**/
|
||||
void pdr_saveGnssInfo(lct_nmea* nmea_data, lct_fs* result, FILE* fp_gps)
|
||||
{
|
||||
lct_nmea_rmc rmc = nmea_data->rmc[1];
|
||||
// 选取更新时间的rmc
|
||||
if (nmea_data->rmc[0].time > nmea_data->rmc[1].time) {
|
||||
rmc = nmea_data->rmc[0];
|
||||
}
|
||||
if (rmc.update && (rmc.status == POSITIONING_Y)) {
|
||||
char str[256];
|
||||
memset(str, 0, sizeof(str));
|
||||
float accuracy = -1.0f;
|
||||
if (fabs(rmc.time - nmea_data->accuracy.time) < 500){
|
||||
accuracy = nmea_data->accuracy.err;
|
||||
}
|
||||
sprintf(str, "%f,%.10f,%d,%.10f,%d,%.10f,%f,%f", rmc.time, rmc.latitude,
|
||||
rmc.rmc_latitude_ns,rmc.longitudinal, rmc.longitudinal_ew, rmc.cog, accuracy,
|
||||
rmc.speed);
|
||||
|
||||
pdr_WriteCsvStr(fp_gps, str);
|
||||
|
||||
result->gpsLat = rmc.latitude;
|
||||
result->gpsLon = rmc.longitudinal;
|
||||
}
|
||||
}
|
||||
|
||||
/**----------------------------------------------------------------------
|
||||
* Function : pdr_libVersion
|
||||
* Description : pdr库版本号
|
||||
* Date : 2020/8/3 logzhan
|
||||
*---------------------------------------------------------------------**/
|
||||
const char* pdr_libVersion(void){
|
||||
return pdrVersion;
|
||||
}
|
||||
|
||||
/**----------------------------------------------------------------------
|
||||
* Function : pdr_closeAlgorithm
|
||||
* Description : 关闭PDR算法,释放资源
|
||||
* Date : 2020/8/3 logzhan
|
||||
*---------------------------------------------------------------------**/
|
||||
void pdr_closeAlgorithm() {
|
||||
freeTrackSmooth();
|
||||
scene_recognition_destroy();
|
||||
}
|
||||
|
|
@ -1,745 +0,0 @@
|
|||
/******************** (C) COPYRIGHT 2020 Geek************************************
|
||||
* File Name : pdr_base.c
|
||||
* Department : Sensor Algorithm Team
|
||||
* Current Version : V1.2
|
||||
* Author : logzhan
|
||||
* Date of Issued : 2021.02.02
|
||||
* Comments : PDR基础功能函数
|
||||
********************************************************************************/
|
||||
/* Header File Including -----------------------------------------------------*/
|
||||
#include <string.h>
|
||||
#include <math.h>
|
||||
#include "pdr_location.h"
|
||||
#include "pdr_kalman.h"
|
||||
#include "pdr_util.h"
|
||||
#include "pdr_ahrs.h"
|
||||
#include "pdr_detector.h"
|
||||
#include "buffer.h"
|
||||
#include "Filter.h"
|
||||
#include "pdr_base.h"
|
||||
#include "pdr_fft.h"
|
||||
|
||||
/* Macro Definition ----------------------------------------------------------*/
|
||||
#define DOWNSAMPLING_RATE 4
|
||||
#define BUF_LEN 256
|
||||
#define ARR_LEN(arr) (sizeof(arr) / sizeof(*arr))
|
||||
#define INC_PTR(buf, ptr) (((ptr) + 1) % ((buf)->length + 1))
|
||||
#define GET_MOL(v) ((float)sqrt((v)[0] * (v)[0] + (v)[1] * (v)[1]))
|
||||
#define P_SET_BIT(msk) (g_pdr.status |= PDR_STATUS_##msk)
|
||||
#define P_CLR_BIT(msk) (g_pdr.status &= ~PDR_STATUS_##msk)
|
||||
#define P_TST_BIT(msk) (g_pdr.status & PDR_STATUS_##msk)
|
||||
|
||||
/* Global Variable Definition ------------------------------------------------*/
|
||||
PDR g_pdr;
|
||||
static DETECTOR *g_detector;
|
||||
static uint64_t g_tick_p;
|
||||
static uint32_t g_motion_type;
|
||||
static uint32_t g_axis_ceof_counter;
|
||||
static BUFFER_LONG g_axis[9];
|
||||
static BUFFER_LONG g_grav[2];
|
||||
static BUFFER_LONG g_erro;
|
||||
static BUFFER_LONG g_posi[4];
|
||||
|
||||
static int angle_count;
|
||||
static double p_angle[3][BUF_LEN];
|
||||
double angle_mean[3] = { 0 };
|
||||
|
||||
static FILTER g_grv_flt[2];
|
||||
|
||||
#if defined(__MATLAB_DBG)
|
||||
static int g_debug[2];
|
||||
static float axis_mean[9];
|
||||
#endif
|
||||
|
||||
const double g_grv_flt_b[] = { 4.62344870721942e-10, 4.62344870721942e-09, 2.08055191824874e-08, 5.54813844866331e-08,
|
||||
9.70924228516079e-08, 1.16510907421929e-07, 9.70924228516079e-08, 5.54813844866331e-08,
|
||||
2.08055191824874e-08, 4.62344870721942e-09, 4.62344870721942e-10 };
|
||||
|
||||
const double g_grv_flt_a[] = { 1, -8.39368255078839, 31.8158646581919, -71.7026569821430, 106.381617694638,
|
||||
-108.553825650279, 77.1444606240244, -37.6960546719428, 12.1197601392885,
|
||||
-2.31493776310228, 0.199454975553812 };
|
||||
const double g_grv_flt_h0 = 1;
|
||||
|
||||
PDR* pdr_initBase() {
|
||||
g_detector = pdr_getDetector();
|
||||
Buffer_initialize((BUFFER *)&g_axis[0], "x_axis_0", BUFFER_TYPE_QUEUE, BUF_LEN);
|
||||
Buffer_initialize((BUFFER *)&g_axis[1], "x_axis_1", BUFFER_TYPE_QUEUE, BUF_LEN);
|
||||
Buffer_initialize((BUFFER *)&g_axis[2], "x_axis_2", BUFFER_TYPE_QUEUE, BUF_LEN);
|
||||
Buffer_initialize((BUFFER *)&g_axis[3], "y_axis_0", BUFFER_TYPE_QUEUE, BUF_LEN);
|
||||
Buffer_initialize((BUFFER *)&g_axis[4], "y_axis_1", BUFFER_TYPE_QUEUE, BUF_LEN);
|
||||
Buffer_initialize((BUFFER *)&g_axis[5], "y_axis_2", BUFFER_TYPE_QUEUE, BUF_LEN);
|
||||
Buffer_initialize((BUFFER *)&g_axis[6], "z_axis_0", BUFFER_TYPE_QUEUE, BUF_LEN);
|
||||
Buffer_initialize((BUFFER *)&g_axis[7], "z_axis_1", BUFFER_TYPE_QUEUE, BUF_LEN);
|
||||
Buffer_initialize((BUFFER *)&g_axis[8], "z_axis_2", BUFFER_TYPE_QUEUE, BUF_LEN);
|
||||
Buffer_initialize((BUFFER *)&g_grav[0], "grav_0", BUFFER_TYPE_QUEUE, BUF_LEN);
|
||||
Buffer_initialize((BUFFER *)&g_grav[1], "grav_1", BUFFER_TYPE_QUEUE, BUF_LEN);
|
||||
Buffer_initialize((BUFFER *)&g_erro, "erro", BUFFER_TYPE_QUEUE, 5 * IMU_SAMPLING_FREQUENCY / DOWNSAMPLING_RATE);
|
||||
Buffer_initialize((BUFFER *)&g_posi[0], "posi_0", BUFFER_TYPE_QUEUE, 60);
|
||||
Buffer_initialize((BUFFER *)&g_posi[1], "posi_1", BUFFER_TYPE_QUEUE, 60);
|
||||
Buffer_initialize((BUFFER *)&g_posi[2], "posi_2", BUFFER_TYPE_QUEUE, 60);
|
||||
Buffer_initialize((BUFFER *)&g_posi[3], "posi_3", BUFFER_TYPE_QUEUE, 60);
|
||||
|
||||
FILTER_setCoef(&g_grv_flt[0], ARR_LEN(g_grv_flt_b), (double *)g_grv_flt_b, (double *)g_grv_flt_a, (double)g_grv_flt_h0);
|
||||
FILTER_setCoef(&g_grv_flt[1], ARR_LEN(g_grv_flt_b), (double *)g_grv_flt_b, (double *)g_grv_flt_a, (double)g_grv_flt_h0);
|
||||
|
||||
pdr_resetData();
|
||||
return &g_pdr;
|
||||
}
|
||||
|
||||
/**----------------------------------------------------------------------
|
||||
* Function : pdr_resetData
|
||||
* Description : 重置PDR变量
|
||||
* Date : 2020/7/21 logzhan
|
||||
*---------------------------------------------------------------------**/
|
||||
void pdr_resetData(void) {
|
||||
memset(&g_pdr, 0, sizeof(g_pdr));
|
||||
g_pdr.status = PDR_STATUS_RESET;
|
||||
g_pdr.axis_ceofficient[0] = 0;
|
||||
g_pdr.axis_ceofficient[1] = 1;
|
||||
g_pdr.axis_ceofficient[2] = 0;
|
||||
g_pdr.bias_direction[0] = 1;
|
||||
g_pdr.bias_direction[1] = 0;
|
||||
g_pdr.pca_accuracy = -1;
|
||||
g_pdr.bias_accuracy = -1;
|
||||
// 初始坐标初始化为0
|
||||
g_pdr.x0 = 0;
|
||||
g_pdr.y0 = 0;
|
||||
// 初始步数为0
|
||||
g_pdr.steps = 0;
|
||||
g_pdr.lastSteps = 0;
|
||||
g_pdr.lastGpsSteps = 0;
|
||||
g_pdr.deltaStepsPerGps = 0;
|
||||
g_pdr.heading = 0.0;
|
||||
g_pdr.lastHeading = 0.0;
|
||||
g_pdr.deltaHeading = 0.0;
|
||||
g_pdr.ts = 0.0;
|
||||
g_pdr.sysStatus = IS_INITIAL;
|
||||
g_pdr.noGpsCount = 0;
|
||||
g_pdr.sceneType = UN_OPEN_AREA;
|
||||
g_pdr.fusionPdrFlg = PDR_FALSE;
|
||||
g_pdr.gpsHeading = 0;
|
||||
g_pdr.trackHeading = ITEM_INVALID;
|
||||
g_pdr.motionFreq = 0.1f;
|
||||
g_pdr.insHeadingOffset = 0.0f;
|
||||
g_pdr.gyroTime = 0.0;
|
||||
memset(g_pdr.pllaInit, 0, sizeof(g_pdr.pllaInit));
|
||||
|
||||
g_tick_p = 0;
|
||||
g_motion_type = g_detector->type;
|
||||
g_axis_ceof_counter = 0;
|
||||
Buffer_clear((BUFFER *)&g_axis[0]);
|
||||
Buffer_clear((BUFFER *)&g_axis[1]);
|
||||
Buffer_clear((BUFFER *)&g_axis[2]);
|
||||
Buffer_clear((BUFFER *)&g_axis[3]);
|
||||
Buffer_clear((BUFFER *)&g_axis[4]);
|
||||
Buffer_clear((BUFFER *)&g_axis[5]);
|
||||
Buffer_clear((BUFFER *)&g_axis[6]);
|
||||
Buffer_clear((BUFFER *)&g_axis[7]);
|
||||
Buffer_clear((BUFFER *)&g_axis[8]);
|
||||
Buffer_clear((BUFFER *)&g_grav[0]);
|
||||
Buffer_clear((BUFFER *)&g_grav[1]);
|
||||
Buffer_clear((BUFFER *)&g_erro);
|
||||
Buffer_clear((BUFFER *)&g_posi[0]);
|
||||
Buffer_clear((BUFFER *)&g_posi[1]);
|
||||
Buffer_clear((BUFFER *)&g_posi[2]);
|
||||
Buffer_clear((BUFFER *)&g_posi[3]);
|
||||
FILTER_reset(&g_grv_flt[0]);
|
||||
FILTER_reset(&g_grv_flt[1]);
|
||||
|
||||
angle_count = 0;
|
||||
memset(&p_angle, 0, sizeof(p_angle));
|
||||
memset(&angle_mean, 0, sizeof(angle_mean));
|
||||
}
|
||||
|
||||
|
||||
static float get_main_frequency(BUFFER_SHORT *frequency, BUFFER_SHORT *amplitude, int number) {
|
||||
float amp[3] = { {0.0f} };
|
||||
float frq;
|
||||
int count;
|
||||
int index;
|
||||
int i;
|
||||
|
||||
if (number > 3) {
|
||||
return 0;
|
||||
}
|
||||
|
||||
for (i = 0; i < number; ++i) {
|
||||
amp[i] = 0;
|
||||
count = 0;
|
||||
for (index = amplitude[i]._bottom; index != amplitude[i]._top; index = INC_PTR(&litude[i], index)) {
|
||||
if (0 != amplitude[i].data[index]) {
|
||||
amp[i] += amplitude[i].data[index];
|
||||
++count;
|
||||
}
|
||||
}
|
||||
if (0 != count) {
|
||||
amp[i] /= count;
|
||||
}
|
||||
}
|
||||
i = amp[0] > amp[1] ? 0 : 1;
|
||||
i = amp[i] > amp[2] ? i : 2;
|
||||
frq = 0;
|
||||
count = 0;
|
||||
for (index = frequency[i]._bottom; index != frequency[i]._top; index = INC_PTR(&frequency[i], index)) {
|
||||
if (0 != frequency[i].data[index]) {
|
||||
frq += frequency[i].data[index];
|
||||
++count;
|
||||
}
|
||||
}
|
||||
if (0 != count) {
|
||||
frq /= count;
|
||||
}
|
||||
|
||||
return frq;
|
||||
}
|
||||
|
||||
static int get_optimal_length_by_frequency(BUFFER_LONG *buffer, float frequency) {
|
||||
int temp;
|
||||
|
||||
if (0 != frequency) {
|
||||
temp = (int)((int)(buffer->length / (IMU_SAMPLING_FREQUENCY / frequency)) * (IMU_SAMPLING_FREQUENCY / frequency));
|
||||
return 0 == temp ? buffer->length : temp;
|
||||
}
|
||||
else {
|
||||
return buffer->length;
|
||||
}
|
||||
}
|
||||
|
||||
static void get_buffer_mean(float *mean, BUFFER_LONG *buffer, int length) {
|
||||
int count;
|
||||
int index;
|
||||
int i;
|
||||
|
||||
count = (buffer->_top - buffer->_bottom + buffer->length + 1) % (buffer->length + 1);
|
||||
length = count < length ? count : length;
|
||||
count = 0;
|
||||
memset(mean, 0, 3 * sizeof(*mean));
|
||||
for (index = (buffer->_top + buffer->length + 1 - length) % (buffer->length + 1); index != buffer->_top; index = INC_PTR(buffer, index)) {
|
||||
for (i = 0; i < 3; ++i) {
|
||||
mean[i] += buffer[i].data[index];
|
||||
}
|
||||
++count;
|
||||
}
|
||||
mean[0] /= count;
|
||||
mean[1] /= -count;
|
||||
mean[2] /= -count;
|
||||
}
|
||||
|
||||
static void get_pca_direction(float *direction, BUFFER_LONG *buffer, int length) {
|
||||
float buff_mean[2];
|
||||
float varX;
|
||||
float varY;
|
||||
float varXY;
|
||||
float lambda;
|
||||
int count;
|
||||
int index;
|
||||
int i;
|
||||
|
||||
count = (buffer->_top - buffer->_bottom + buffer->length + 1) % (buffer->length + 1);
|
||||
length = count < length ? count : length;
|
||||
memset(buff_mean, 0, sizeof(buff_mean));
|
||||
for (index = (buffer[0]._top + buffer[0].length + 1 - length) % (buffer[0].length + 1); index != buffer[0]._top; index = INC_PTR(&buffer[0], index)) {
|
||||
for (i = 0; i < 2; ++i) {
|
||||
buff_mean[i] += buffer[i].data[index];
|
||||
}
|
||||
}
|
||||
buff_mean[0] /= length;
|
||||
buff_mean[1] /= length;
|
||||
varX = 0;
|
||||
varY = 0;
|
||||
varXY = 0;
|
||||
for (index = (buffer[0]._top + buffer[0].length + 1 - length) % (buffer[0].length + 1); index != buffer[0]._top; index = INC_PTR(&buffer[0], index)) {
|
||||
float deltaX = buffer[0].data[index] - buff_mean[0];
|
||||
float deltaY = buffer[1].data[index] - buff_mean[1];
|
||||
varX += deltaX * deltaX;
|
||||
varY += deltaY * deltaY;
|
||||
varXY += deltaX * deltaY;
|
||||
}
|
||||
lambda = ((varX + varY) + (float)sqrt((varX - varY) * (varX - varY) + 4 * varXY * varXY)) * 0.5f;
|
||||
if (0 != varXY) {
|
||||
direction[0] = (lambda - varY) / varXY;
|
||||
direction[1] = InvSqrt(direction[0] * direction[0] + 1);
|
||||
direction[0] *= direction[1];
|
||||
direction[1] = -direction[1];
|
||||
}
|
||||
else {
|
||||
direction[0] = 0;
|
||||
direction[1] = 0;
|
||||
}
|
||||
}
|
||||
|
||||
static void caculate_determinant(float *determinant, float A[][3], int row, int col) {
|
||||
int k = 0;
|
||||
float temp[4];
|
||||
for (uchar i = 0; i < 3; ++i) {
|
||||
if (row == i) continue;
|
||||
for (uchar j = 0; j < 3; ++j) {
|
||||
if (col == j) continue;
|
||||
temp[k++] = A[i][j];
|
||||
}
|
||||
}
|
||||
*determinant = temp[0] * temp[3] - temp[1] * temp[2];
|
||||
}
|
||||
|
||||
static void caculate_adjoint_matrix(float A_star[][3], float A[][3]) {
|
||||
int i;
|
||||
int j;
|
||||
|
||||
for (i = 0; i < 3; ++i) {
|
||||
for (j = 0; j < 3; ++j) {
|
||||
caculate_determinant(&A_star[i][j], A, j, i);
|
||||
if ((i + j) % 2) {
|
||||
A_star[i][j] = -A_star[i][j];
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
static void get_ceof_by_pca(float *ceof, float *axis_mean, float *pca_direction) {
|
||||
float A[3][3];
|
||||
float A_star[3][3];
|
||||
float x[3];
|
||||
float axs_dir[2];
|
||||
float mold;
|
||||
int dir;
|
||||
int i;
|
||||
int j;
|
||||
|
||||
/* caculate adjoint matrix */
|
||||
for (i = 0; i < 3; ++i) {
|
||||
for (j = 0; j < 3; ++j) {
|
||||
A[i][j] = axis_mean[j * 3 + i];
|
||||
}
|
||||
}
|
||||
caculate_adjoint_matrix(A_star, A);
|
||||
|
||||
/* caculate x vector (Ax = b) */
|
||||
for (i = 0; i < 3; ++i) {
|
||||
x[i] = A_star[i][0] * pca_direction[0] + A_star[i][1] * pca_direction[1];
|
||||
}
|
||||
mold = (float)sqrt(x[0] * x[0] + x[1] * x[1] + x[2] * x[2]);
|
||||
if (0 == mold) {
|
||||
goto GET_CEOF_BY_PCA_ARITHMETIC_ERROR;
|
||||
}
|
||||
for (i = 0; i < 3; ++i) {
|
||||
x[i] /= mold;
|
||||
}
|
||||
|
||||
for (i = 0; i < 2; ++i) {
|
||||
axs_dir[i] = A[i][0] * ceof[0] + A[i][1] * ceof[1] + A[i][2] * ceof[2];
|
||||
}
|
||||
dir = (axs_dir[0] * pca_direction[0] + axs_dir[1] * pca_direction[1] < 0) ? -1 : 1;
|
||||
|
||||
for (i = 0; i < 3; ++i) {
|
||||
ceof[i] = dir * x[i];
|
||||
}
|
||||
|
||||
return;
|
||||
|
||||
GET_CEOF_BY_PCA_ARITHMETIC_ERROR:
|
||||
ceof[0] = 0;
|
||||
ceof[1] = 1;
|
||||
ceof[2] = 0;
|
||||
}
|
||||
|
||||
static void correct_axis_ceofficient(float ratio) {
|
||||
float axis_mean[9];
|
||||
float pca_direction[2];
|
||||
float axis_ceofficient[3];
|
||||
float mold;
|
||||
int length;
|
||||
int i;
|
||||
|
||||
length = get_optimal_length_by_frequency(g_axis, g_pdr.motionFreq);
|
||||
get_buffer_mean(&axis_mean[0], &g_axis[0], length);
|
||||
get_buffer_mean(&axis_mean[3], &g_axis[3], length);
|
||||
get_buffer_mean(&axis_mean[6], &g_axis[6], length);
|
||||
get_pca_direction(pca_direction, g_grav, length);
|
||||
memcpy((void *)axis_ceofficient, (void *)g_pdr.axis_ceofficient, sizeof(g_pdr.axis_ceofficient));
|
||||
get_ceof_by_pca(axis_ceofficient, axis_mean, pca_direction);
|
||||
for (i = 0; i < 3; ++i) {
|
||||
g_pdr.axis_ceofficient[i] = (1 - ratio) * g_pdr.axis_ceofficient[i] + ratio * axis_ceofficient[i];
|
||||
}
|
||||
mold = (float)sqrt(g_pdr.axis_ceofficient[0] * g_pdr.axis_ceofficient[0] + g_pdr.axis_ceofficient[1] * g_pdr.axis_ceofficient[1] + g_pdr.axis_ceofficient[2] * g_pdr.axis_ceofficient[2]);
|
||||
for (i = 0; i < 3; ++i) {
|
||||
g_pdr.axis_ceofficient[i] /= mold;
|
||||
}
|
||||
}
|
||||
|
||||
/**----------------------------------------------------------------------
|
||||
* Function : pdr_computePCA
|
||||
* Description : 进入RESET_PHASE_1\STABLE阶段之后进行PCA计算
|
||||
* Date : 2020/7/21 logzhan
|
||||
*---------------------------------------------------------------------**/
|
||||
void pdr_computePCA(AHRS *ahrs) {
|
||||
float axis_mean[9];
|
||||
float freq;
|
||||
float mold;
|
||||
int length;
|
||||
int i;
|
||||
double attitude[3];
|
||||
|
||||
double angle_sum[3] = { 0 };
|
||||
int angle_len;
|
||||
|
||||
//RESET_1时只加ticker，之后直接返回
|
||||
/* filter bad data */
|
||||
if (AHRS_STATUS_RESET & ahrs->status) {
|
||||
++g_tick_p;
|
||||
return;
|
||||
}
|
||||
/* caculate axis ceofficient */
|
||||
/*PDR_STATUS_CACULATING_AXIS_CEOFFICIENT 0x40*/
|
||||
if (P_TST_BIT(CACULATING_AXIS_CEOFFICIENT) && \
|
||||
(g_grav[0]._top - g_grav[0]._bottom + g_grav[0].length + 1) % (g_grav[0].length + 1) > (BUF_LEN >> 1)) {
|
||||
|
||||
correct_axis_ceofficient(0.50f);
|
||||
P_CLR_BIT(CACULATING_AXIS_CEOFFICIENT);
|
||||
P_SET_BIT(PCA_STABLE);
|
||||
|
||||
g_erro._bottom = 0;
|
||||
for (g_erro._top = 0; g_erro._top < g_erro.length && g_erro._top < 2 * IMU_SAMPLING_FREQUENCY / DOWNSAMPLING_RATE; ++g_erro._top) {
|
||||
g_erro.data[g_erro._top] = (g_erro._top % 2) ? +0.4f : -0.4f;
|
||||
}
|
||||
Buffer_clear((BUFFER *)&g_posi[0]);
|
||||
Buffer_clear((BUFFER *)&g_posi[1]);
|
||||
Buffer_clear((BUFFER *)&g_posi[2]);
|
||||
Buffer_clear((BUFFER *)&g_posi[3]);
|
||||
g_pdr.bias_direction[0] = 1;
|
||||
g_pdr.bias_direction[1] = 0;
|
||||
g_pdr.bias_accuracy = -1;
|
||||
//++flag;
|
||||
//}
|
||||
}
|
||||
|
||||
/* correct axis ceofficient */
|
||||
if ((g_pdr.status & PDR_STATUS_PCA_STABLE) && 0 == (g_tick_p % (2 * IMU_SAMPLING_FREQUENCY))) {
|
||||
correct_axis_ceofficient(0.10f);
|
||||
}
|
||||
|
||||
/* downsample */
|
||||
if (0 != (g_tick_p % DOWNSAMPLING_RATE)) {
|
||||
++g_tick_p;
|
||||
return;
|
||||
}
|
||||
|
||||
/* push data */
|
||||
for (i = 0; i < 3; ++i) {
|
||||
buffer_push((BUFFER *)&g_axis[0 + i], ahrs->x_axis[i]);
|
||||
buffer_push((BUFFER *)&g_axis[3 + i], ahrs->y_axis[i]);
|
||||
buffer_push((BUFFER *)&g_axis[6 + i], ahrs->z_axis[i]);
|
||||
}
|
||||
for (i = 0; i < 2; ++i) {
|
||||
//Buffer_push((BUFFER *)&g_grav[i], (float)FILTER_filter(&g_grv_flt[i], ahrs->gravity[i]));
|
||||
buffer_push((BUFFER *)&g_grav[i], ahrs->gravity[i]);
|
||||
}
|
||||
|
||||
/* motion frequency */
|
||||
freq = get_main_frequency(g_acc_frq_buf, g_acc_amp_buf, 3);
|
||||
if (3.5 >= freq && freq > 1.5) {
|
||||
g_pdr.motionFreq = freq;
|
||||
}
|
||||
else if (1.5 >= freq && freq > 0.75){
|
||||
g_pdr.motionFreq = 2 * freq;
|
||||
}
|
||||
|
||||
/* axis direction */
|
||||
length = get_optimal_length_by_frequency(g_axis, g_pdr.motionFreq);
|
||||
get_buffer_mean(&axis_mean[0], &g_axis[0], length);
|
||||
get_buffer_mean(&axis_mean[3], &g_axis[3], length);
|
||||
get_buffer_mean(&axis_mean[6], &g_axis[6], length);
|
||||
memset(g_pdr.axis_direction, 0, sizeof(g_pdr.axis_direction));
|
||||
for (i = 0; i < 2; ++i) {
|
||||
g_pdr.axis_direction[i] = g_pdr.axis_ceofficient[0] * axis_mean[0 + i] + g_pdr.axis_ceofficient[1] * axis_mean[3 + i] + g_pdr.axis_ceofficient[2] * axis_mean[6 + i];
|
||||
}
|
||||
mold = (float)GET_MOL(g_pdr.axis_direction);
|
||||
if (0 != mold) {
|
||||
g_pdr.axis_direction[0] /= mold;
|
||||
g_pdr.axis_direction[1] /= mold;
|
||||
}
|
||||
|
||||
/* pca direction */
|
||||
get_pca_direction(g_pdr.pca_direction, g_grav, length);
|
||||
if (g_pdr.pca_direction[0] * g_pdr.axis_direction[0] + g_pdr.pca_direction[1] * g_pdr.axis_direction[1] < 0) {
|
||||
g_pdr.pca_direction[0] = -g_pdr.pca_direction[0];
|
||||
g_pdr.pca_direction[1] = -g_pdr.pca_direction[1];
|
||||
}
|
||||
|
||||
/* direction accuracy */
|
||||
/*代码疑问1：g_erro为什么由下式确定，什么含义*/
|
||||
buffer_push((BUFFER *)&g_erro, (float)fabs(g_pdr.axis_direction[0] * g_pdr.pca_direction[1] - g_pdr.axis_direction[1] * g_pdr.pca_direction[0]));
|
||||
if (!P_TST_BIT(CACULATING_AXIS_CEOFFICIENT)) {
|
||||
float accuracy;
|
||||
|
||||
Buffer_std((BUFFER *)&g_erro, &accuracy);
|
||||
if (DETECTOR_TYPE_HANDHELD == g_pdr.motionType) {
|
||||
if (!P_TST_BIT(STABLE) && !P_TST_BIT(CACULATING_AXIS_CEOFFICIENT) && 0.15 >= accuracy) {
|
||||
P_SET_BIT(CACULATING_AXIS_CEOFFICIENT);
|
||||
}
|
||||
else if (g_pdr.pca_accuracy < 0.30 && 0.30 <= accuracy) {
|
||||
P_CLR_BIT(CACULATING_AXIS_CEOFFICIENT);
|
||||
P_CLR_BIT(STABLE);
|
||||
}
|
||||
}
|
||||
g_pdr.pca_accuracy = accuracy;
|
||||
}
|
||||
else {
|
||||
g_pdr.pca_accuracy = -1;
|
||||
}
|
||||
|
||||
/* calibrated direction */
|
||||
g_pdr.cal_direction[0] = g_pdr.pca_direction[0] * g_pdr.bias_direction[0] + g_pdr.pca_direction[1] * g_pdr.bias_direction[1];
|
||||
g_pdr.cal_direction[1] = g_pdr.pca_direction[1] * g_pdr.bias_direction[0] - g_pdr.pca_direction[0] * g_pdr.bias_direction[1];
|
||||
|
||||
//PDR_LOGD("%f %f\n", g_pdr.cal_direction[0],g_pdr.cal_direction[1]);
|
||||
|
||||
if (DETECTOR_TYPE_STATIC == g_detector->type || DETECTOR_TYPE_IRREGULAR == g_detector->type) {
|
||||
g_pdr.motionFreq = 0;
|
||||
}
|
||||
|
||||
Qnb2Att(ahrs->qnb,attitude);
|
||||
|
||||
if (angle_count < BUF_LEN - 1)
|
||||
{
|
||||
p_angle[0][angle_count] = attitude[0];
|
||||
p_angle[1][angle_count] = attitude[1];
|
||||
p_angle[2][angle_count] = attitude[2];
|
||||
angle_count++;
|
||||
}
|
||||
else{
|
||||
for (int i = 0; i < BUF_LEN - 1; i++)
|
||||
{
|
||||
p_angle[0][i] = p_angle[0][i + 1];
|
||||
p_angle[1][i] = p_angle[1][i + 1];
|
||||
p_angle[2][i] = p_angle[2][i + 1];
|
||||
}
|
||||
p_angle[0][BUF_LEN - 1] = attitude[0];
|
||||
p_angle[1][BUF_LEN - 1] = attitude[1];
|
||||
p_angle[2][BUF_LEN - 1] = attitude[2];
|
||||
|
||||
angle_len = (length <= BUF_LEN) ? length : BUF_LEN;
|
||||
for (int j = BUF_LEN - 1; j >(BUF_LEN - 1 - angle_len); j--)
|
||||
{
|
||||
angle_sum[0] = angle_sum[0] + p_angle[0][j];
|
||||
angle_sum[1] = angle_sum[1] + p_angle[1][j];
|
||||
angle_sum[2] = angle_sum[2] + p_angle[2][j];
|
||||
}
|
||||
angle_mean[0] = angle_sum[0] / angle_len;
|
||||
angle_mean[1] = angle_sum[1] / angle_len;
|
||||
angle_mean[2] = angle_sum[2] / angle_len;
|
||||
}
|
||||
|
||||
++g_tick_p;
|
||||
}
|
||||
|
||||
/**----------------------------------------------------------------------
|
||||
* Function : pdr_outputGpsPos
|
||||
* Description : PDR输出原始GPS定位结果
|
||||
* Date : 2020/07/08 logzhan
|
||||
*---------------------------------------------------------------------**/
|
||||
void pdr_outputGpsPos(gnss* pgnss, lct_fs* result) {
|
||||
result->latitude = pgnss->lat;
|
||||
result->latitude_ns = pgnss->lat_ns;
|
||||
result->longitudinal = pgnss->lon;
|
||||
result->longitudinal_ew = pgnss->lon_ew;
|
||||
result->time = pgnss->timestamps;
|
||||
}
|
||||
|
||||
|
||||
/**----------------------------------------------------------------------
|
||||
* Function : pdr_detStatic
|
||||
* Description : 检测用户是否处于静止状态, 目前主要采用计步器判断和运动频率分析
|
||||
* 单运动频率计算量太大，可以考虑用加速度判断用户状态。
|
||||
* Date : 2021/01/28 logzhan
|
||||
*---------------------------------------------------------------------**/
|
||||
int pdr_detStatic(PDR* g_pdr, gnss* pgnss, unsigned long delSteps) {
|
||||
|
||||
if (pgnss->vel != -1 && pgnss->vel < 1 && (delSteps == 0 &&
|
||||
g_pdr->motionFreq == 0.0)) {
|
||||
return PDR_TRUE;
|
||||
}
|
||||
return PDR_FALSE;
|
||||
}
|
||||
|
||||
/**----------------------------------------------------------------------
|
||||
* Function : detIsCarMode
|
||||
* Description : 识别是否是车载模式
|
||||
* Input : pgnss，GPS数据结构体
|
||||
g_pdr，PDR结构体
|
||||
delSteps， 与上次相比步数的变化量
|
||||
time，计数器
|
||||
* Output : int，车载模式标志位
|
||||
* Date : 2021/01/28 logzhan
|
||||
*---------------------------------------------------------------------**/
|
||||
int detIsCarMode(gnss* pgnss, PDR* g_pdr, unsigned long delSteps, int* time) {
|
||||
if ((pgnss->vel > 10 && delSteps == 0) ||
|
||||
(delSteps == 0 && g_pdr->motionFreq == 0.0 && pgnss->vel > 3 && pgnss->error < 30))
|
||||
{
|
||||
(*time)++;
|
||||
}else {
|
||||
*time = 0;
|
||||
}
|
||||
if (*time > 5) {
|
||||
return PDR_TRUE;
|
||||
}
|
||||
return PDR_FALSE;
|
||||
}
|
||||
|
||||
/**----------------------------------------------------------------------
|
||||
* Function : detPdrToReset
|
||||
* Description : 检测PDR系统是否需要重置，考虑到后续PDR推算失误，或者其他情况
|
||||
* 重置PDR到GPS的位置。
|
||||
* Date : 2021/01/28 logzhan
|
||||
*---------------------------------------------------------------------**/
|
||||
int detPdrToReset(double pdr_angle, int* gpscnt, ulong deltsteps, PDR* g_pdr) {
|
||||
|
||||
if ((pdr_angle < 0 && deltsteps > 0) || (deltsteps > 0 && (g_pdr->motionFreq == 0))) {
|
||||
(*gpscnt)++;
|
||||
}else {
|
||||
*gpscnt = 0;
|
||||
}
|
||||
return (*gpscnt > 6);
|
||||
}
|
||||
|
||||
/**---------------------------------------------------------------------
|
||||
* Function : detectorUpdate
|
||||
* Description : 根据新确定的手机携带方式更新g_pdr.status等
|
||||
* Date : 2020/7/20 logzhan
|
||||
*---------------------------------------------------------------------**/
|
||||
void detectorUpdate(DETECTOR *detector) {
|
||||
|
||||
// 设置PDR的运动类型
|
||||
g_pdr.motionType = detector->type;
|
||||
|
||||
if (DETECTOR_TYPE_SWINGING == g_pdr.motionType && DETECTOR_TYPE_SWINGING != g_motion_type) {
|
||||
if (!P_TST_BIT(STABLE)) {
|
||||
P_SET_BIT(CACULATING_AXIS_CEOFFICIENT);
|
||||
P_CLR_BIT(STABLE);
|
||||
}
|
||||
}
|
||||
else if ((DETECTOR_TYPE_STATIC == g_pdr.motionType || DETECTOR_TYPE_IRREGULAR == g_pdr.motionType) &&
|
||||
(DETECTOR_TYPE_HANDHELD == g_motion_type || DETECTOR_TYPE_SWINGING == g_motion_type)) {
|
||||
P_CLR_BIT(CACULATING_AXIS_CEOFFICIENT);
|
||||
P_CLR_BIT(STABLE);
|
||||
}
|
||||
|
||||
g_motion_type = g_pdr.motionType;
|
||||
}
|
||||
|
||||
/**---------------------------------------------------------------------
|
||||
* Function : gpsUpdateCb
|
||||
* Description : gps更新回调函数，目前初步来看主要是计算PCA和GPS方向的区别，
|
||||
* 分析PCA方向误差作用。
|
||||
* Date : 2020/02/16 logzhan
|
||||
*---------------------------------------------------------------------**/
|
||||
void gpsUpdateCb(gnss* gps)
|
||||
{
|
||||
float temp;
|
||||
float meanA[2];
|
||||
float meanY[2];
|
||||
float ctc; // ctc[0][0], ctc[1][1]
|
||||
float cty[2];
|
||||
float a[2];
|
||||
float mold;
|
||||
float dx;
|
||||
float dy;
|
||||
int count;
|
||||
int index;
|
||||
|
||||
/* filter bad point */
|
||||
double vel_thr;
|
||||
double error_thr;
|
||||
if (g_pdr.sceneType) {
|
||||
vel_thr = 2.5;
|
||||
error_thr = 6;
|
||||
}
|
||||
else {
|
||||
vel_thr = 1.0;
|
||||
error_thr = 15;
|
||||
}
|
||||
|
||||
if (fabs(gps->timestamps - g_pdr.gyroTime) > 1000
|
||||
|| gps->vel < vel_thr \
|
||||
|| gps->error > error_thr \
|
||||
|| (DETECTOR_TYPE_SWINGING != g_detector->type && DETECTOR_TYPE_HANDHELD != g_detector->type) \
|
||||
|| -1 == g_pdr.pca_accuracy \
|
||||
|| gps->yaw < 0) {
|
||||
return;
|
||||
}
|
||||
|
||||
/* caculate position */
|
||||
Buffer_top((BUFFER *)&g_posi[0], &temp);
|
||||
buffer_push((BUFFER *)&g_posi[0], temp + (float)cos(gps->yaw / 180 * M_PI));
|
||||
Buffer_top((BUFFER *)&g_posi[1], &temp);
|
||||
buffer_push((BUFFER *)&g_posi[1], temp + (float)sin(gps->yaw / 180 * M_PI));
|
||||
Buffer_top((BUFFER *)&g_posi[2], &temp);
|
||||
buffer_push((BUFFER *)&g_posi[2], temp + g_pdr.pca_direction[0]);
|
||||
Buffer_top((BUFFER *)&g_posi[3], &temp);
|
||||
buffer_push((BUFFER *)&g_posi[3], temp + g_pdr.pca_direction[1]);
|
||||
if ((g_posi[0]._top - g_posi[0]._bottom + g_posi[0].length + 1) % (g_posi[0].length + 1) >= 5) {
|
||||
|
||||
count = 0;
|
||||
meanA[0] = 0;
|
||||
meanA[1] = 0;
|
||||
meanY[0] = 0;
|
||||
meanY[1] = 0;
|
||||
for (index = g_posi[0]._bottom; index != g_posi[0]._top; index = INC_PTR(&g_posi[0], index)) {
|
||||
meanA[0] += g_posi[0].data[index];
|
||||
meanA[1] += g_posi[1].data[index];
|
||||
meanY[0] += g_posi[2].data[index];
|
||||
meanY[1] += g_posi[3].data[index];
|
||||
++count;
|
||||
}
|
||||
meanA[0] /= count;
|
||||
meanA[1] /= count;
|
||||
meanY[0] /= count;
|
||||
meanY[1] /= count;
|
||||
|
||||
for (index = g_posi[0]._bottom; index != g_posi[0]._top; index = INC_PTR(&g_posi[0], index)) {
|
||||
g_posi[0].data[index] -= meanA[0];
|
||||
g_posi[1].data[index] -= meanA[1];
|
||||
g_posi[2].data[index] -= meanY[0];
|
||||
g_posi[3].data[index] -= meanY[1];
|
||||
}
|
||||
|
||||
ctc = 0;
|
||||
cty[0] = 0;
|
||||
cty[1] = 0;
|
||||
for (index = g_posi[0]._bottom; index != g_posi[0]._top; index = INC_PTR(&g_posi[0], index)) {
|
||||
ctc += g_posi[0].data[index] * g_posi[0].data[index] + g_posi[1].data[index] * g_posi[1].data[index];
|
||||
cty[0] += g_posi[0].data[index] * g_posi[2].data[index] + g_posi[1].data[index] * g_posi[3].data[index];
|
||||
cty[1] += -g_posi[1].data[index] * g_posi[2].data[index] + g_posi[0].data[index] * g_posi[3].data[index];
|
||||
}
|
||||
|
||||
if (0 != ctc) {
|
||||
/* (c^Tc)^{-1}c^Ty */
|
||||
a[0] = cty[0] / ctc;
|
||||
a[1] = cty[1] / ctc;
|
||||
}
|
||||
else {
|
||||
goto GPS_UPDATE_ARITHMETIC_ERROR;
|
||||
}
|
||||
|
||||
if (0 == (mold = (float)GET_MOL(a))) {
|
||||
goto GPS_UPDATE_ARITHMETIC_ERROR;
|
||||
}
|
||||
g_pdr.bias_direction[0] = a[0] / mold;
|
||||
g_pdr.bias_direction[1] = a[1] / mold;
|
||||
g_pdr.bias_accuracy = 0;
|
||||
for (index = g_posi[0]._bottom; index != g_posi[0]._top; index = INC_PTR(&g_posi[0], index)) {
|
||||
dx = g_posi[2].data[index] - a[0] * g_posi[0].data[index] + a[1] * g_posi[1].data[index];
|
||||
dy = g_posi[3].data[index] - a[1] * g_posi[0].data[index] - a[0] * g_posi[1].data[index];
|
||||
g_pdr.bias_accuracy += dx * dx + dy * dy;
|
||||
}
|
||||
g_pdr.bias_accuracy = (float)sqrt(g_pdr.bias_accuracy / count);
|
||||
P_SET_BIT(BIAS_STABLE);
|
||||
|
||||
return;
|
||||
}
|
||||
|
||||
GPS_UPDATE_ARITHMETIC_ERROR:
|
||||
g_pdr.bias_direction[0] = 1;
|
||||
g_pdr.bias_direction[1] = 0;
|
||||
g_pdr.bias_accuracy = -1;
|
||||
P_CLR_BIT(BIAS_STABLE);
|
||||
}
|
||||
|
|
@ -1,875 +0,0 @@
|
|||
/******************** (C) COPYRIGHT 2020 Geek************************************
|
||||
* File Name : pdr_detector.c
|
||||
* Department : Sensor Algorithm Team
|
||||
* Current Version : V1.2
|
||||
* Author : ZhangJingrui@vivo.com
|
||||
* Date of Issued : 2020.7.4
|
||||
* Comments : PDR检测器，主要用于区分用户的运动类型，用于PDR算法进行下一步的
|
||||
* 决策。
|
||||
********************************************************************************/
|
||||
/* Header File Including -----------------------------------------------------*/
|
||||
#include <string.h>
|
||||
#include <math.h>
|
||||
#include <stdio.h>
|
||||
#include "pdr_base.h"
|
||||
#include "pdr_detector.h"
|
||||
#include "Filter.h"
|
||||
#include "buffer.h"
|
||||
#include "pdr_fft.h"
|
||||
#include "m_munkres.h"
|
||||
|
||||
|
||||
/* Macro Definition ----------------------------------------------------------*/
|
||||
#define FLT_BUF_LEN 256
|
||||
#define FRQ_BUF_LEN 10
|
||||
#define TMP_BUF_LEN 5
|
||||
#define FRQ_RAT 0.3f
|
||||
#define ACCE 0
|
||||
#define GYRO 1
|
||||
#define X_AXIS 0
|
||||
#define Y_AXIS 1
|
||||
#define Z_AXIS 2
|
||||
#define ARR_LEN(arr) (sizeof(arr) / sizeof(*arr))
|
||||
#define GET_MOL(x) (sqrt(x[0] * x[0] + x[1] * x[1] + x[2] * x[2]))
|
||||
#define INC_PTR(buf, ptr) (((ptr) + 1) % ((buf)->length + 1))
|
||||
#define DEC_PTR(buf, ptr) (((ptr) + (buf)->length) % ((buf)->length + 1))
|
||||
#define COST(i, j) cost[(i) + (j) * number]
|
||||
#define CALIBRATE_ON 1
|
||||
#define CALIBRATE_OFF 0
|
||||
|
||||
extern int debugCount;
|
||||
static float ellipsoidPoint[3];
|
||||
static float ellipsoidScale[3];
|
||||
|
||||
/* Global Variable Definition ------------------------------------------------*/
|
||||
static DETECTOR g_detector;
|
||||
static BUFFER_LONG g_mol_buf[2];
|
||||
static FILTER g_mol_flt[2];
|
||||
|
||||
const double g_mol_flt_b[] = { 4.96135120696701e-05, 0.000496135120696701, 0.00223260804313515, 0.00595362144836041,
|
||||
0.0104188375346307, 0.0125026050415569, 0.0104188375346307, 0.00595362144836041, 0.00223260804313515,
|
||||
0.000496135120696701, 4.96135120696701e-05 };
|
||||
const double g_mol_flt_a[] = { 1, -3.98765436730188, 8.09440659266207, -10.4762753571126, 9.42333716215302, -6.08421408355183,
|
||||
2.83526165426630, -0.936403462626094, 0.208912324670401, -0.0283358586690797, 0.00176963186900233 };
|
||||
const double g_mol_flt_h0 = 1;
|
||||
BUFFER_SHORT g_acc_frq_buf[3];
|
||||
BUFFER_SHORT g_acc_amp_buf[3];
|
||||
static BUFFER_SHORT g_acc_tmp_buf[3];
|
||||
BUFFER_SHORT g_gyr_frq_buf[3];
|
||||
BUFFER_SHORT g_gyr_amp_buf[3];
|
||||
static BUFFER_SHORT g_gyr_tmp_buf[3];
|
||||
|
||||
static float mag_buff[MAG_BUF_LEN][3];
|
||||
static int mag_count;
|
||||
static unsigned char mag_calibrate;
|
||||
//static double mag_time;
|
||||
static int g_label[5] = { 0 };
|
||||
|
||||
/* Function Definition -------------------------------------------------------------------------- */
|
||||
|
||||
/**---------------------------------------------------------------------
|
||||
* Function : pdr_getDetector
|
||||
* Description : 获取PDR运动分类器的结构体指针
|
||||
* Date : 2020/02/16 logzhan &
|
||||
*
|
||||
*
|
||||
*---------------------------------------------------------------------**/
|
||||
DETECTOR* pdr_getDetector(void) {
|
||||
return &g_detector;
|
||||
}
|
||||
|
||||
/**---------------------------------------------------------------------
|
||||
* Function : pdr_initDetector
|
||||
* Description : 初始化运动分类器，识别用户处于哪一种运动模式
|
||||
* Date : 2020/02/16 logzhan & logzhan
|
||||
*---------------------------------------------------------------------**/
|
||||
DETECTOR* pdr_initDetector(void) {
|
||||
pdr_resetDetector();
|
||||
return &g_detector;
|
||||
}
|
||||
|
||||
/**---------------------------------------------------------------------
|
||||
* Function : pdr_resetDetector
|
||||
* Description : 重置PDR运动分类器
|
||||
* Date : 2020/02/16 logzhan & logzhan
|
||||
*---------------------------------------------------------------------**/
|
||||
void pdr_resetDetector(void) {
|
||||
|
||||
g_detector.tick = 0;
|
||||
g_detector.lastType = DETECTOR_TYPE_STATIC;
|
||||
|
||||
Buffer_initialize((BUFFER *)&g_mol_buf[ACCE], "acce_mold_filtered", BUFFER_TYPE_QUEUE, FLT_BUF_LEN);
|
||||
Buffer_initialize((BUFFER *)&g_mol_buf[GYRO], "gyro_mold_filtered", BUFFER_TYPE_QUEUE, FLT_BUF_LEN);
|
||||
Buffer_initialize((BUFFER *)&g_acc_frq_buf[0], "acc_frq_buf_0", BUFFER_TYPE_QUEUE, FRQ_BUF_LEN);
|
||||
Buffer_initialize((BUFFER *)&g_acc_frq_buf[1], "acc_frq_buf_1", BUFFER_TYPE_QUEUE, FRQ_BUF_LEN);
|
||||
Buffer_initialize((BUFFER *)&g_acc_frq_buf[2], "acc_frq_buf_2", BUFFER_TYPE_QUEUE, FRQ_BUF_LEN);
|
||||
Buffer_initialize((BUFFER *)&g_acc_amp_buf[0], "acc_amp_buf_0", BUFFER_TYPE_QUEUE, FRQ_BUF_LEN);
|
||||
Buffer_initialize((BUFFER *)&g_acc_amp_buf[1], "acc_amp_buf_1", BUFFER_TYPE_QUEUE, FRQ_BUF_LEN);
|
||||
Buffer_initialize((BUFFER *)&g_acc_amp_buf[2], "acc_amp_buf_2", BUFFER_TYPE_QUEUE, FRQ_BUF_LEN);
|
||||
Buffer_initialize((BUFFER *)&g_acc_tmp_buf[0], "acc_tmp_buf_0", BUFFER_TYPE_QUEUE, TMP_BUF_LEN);
|
||||
Buffer_initialize((BUFFER *)&g_acc_tmp_buf[1], "acc_tmp_buf_1", BUFFER_TYPE_QUEUE, TMP_BUF_LEN);
|
||||
Buffer_initialize((BUFFER *)&g_acc_tmp_buf[2], "acc_tmp_buf_2", BUFFER_TYPE_QUEUE, TMP_BUF_LEN);
|
||||
Buffer_initialize((BUFFER *)&g_gyr_frq_buf[0], "gyr_frq_buf_0", BUFFER_TYPE_QUEUE, FRQ_BUF_LEN);
|
||||
Buffer_initialize((BUFFER *)&g_gyr_frq_buf[1], "gyr_frq_buf_1", BUFFER_TYPE_QUEUE, FRQ_BUF_LEN);
|
||||
Buffer_initialize((BUFFER *)&g_gyr_frq_buf[2], "gyr_frq_buf_2", BUFFER_TYPE_QUEUE, FRQ_BUF_LEN);
|
||||
Buffer_initialize((BUFFER *)&g_gyr_amp_buf[0], "gyr_amp_buf_0", BUFFER_TYPE_QUEUE, FRQ_BUF_LEN);
|
||||
Buffer_initialize((BUFFER *)&g_gyr_amp_buf[1], "gyr_amp_buf_1", BUFFER_TYPE_QUEUE, FRQ_BUF_LEN);
|
||||
Buffer_initialize((BUFFER *)&g_gyr_amp_buf[2], "gyr_amp_buf_2", BUFFER_TYPE_QUEUE, FRQ_BUF_LEN);
|
||||
Buffer_initialize((BUFFER *)&g_gyr_tmp_buf[0], "gyr_tmp_buf_0", BUFFER_TYPE_QUEUE, TMP_BUF_LEN);
|
||||
Buffer_initialize((BUFFER *)&g_gyr_tmp_buf[1], "gyr_tmp_buf_1", BUFFER_TYPE_QUEUE, TMP_BUF_LEN);
|
||||
Buffer_initialize((BUFFER *)&g_gyr_tmp_buf[2], "gyr_tmp_buf_2", BUFFER_TYPE_QUEUE, TMP_BUF_LEN);
|
||||
FILTER_setCoef(&g_mol_flt[ACCE], ARR_LEN(g_mol_flt_b), (double *)g_mol_flt_b, (double *)g_mol_flt_a, (double)g_mol_flt_h0);
|
||||
FILTER_setCoef(&g_mol_flt[GYRO], ARR_LEN(g_mol_flt_b), (double *)g_mol_flt_b, (double *)g_mol_flt_a, (double)g_mol_flt_h0);
|
||||
memset(*mag_buff,0,sizeof(mag_buff));
|
||||
mag_count = 0;
|
||||
mag_calibrate = CALIBRATE_OFF;
|
||||
ellipsoidPoint[0] = 0;
|
||||
ellipsoidPoint[1] = 0;
|
||||
ellipsoidPoint[2] = 0;
|
||||
ellipsoidScale[0] = 1;
|
||||
ellipsoidScale[1] = 1;
|
||||
ellipsoidScale[2] = 1;
|
||||
|
||||
memset((void *)g_label, 0, sizeof(g_label));
|
||||
}
|
||||
|
||||
|
||||
/**---------------------------------------------------------------------
|
||||
* Function : updateDetectorImu
|
||||
* Description : 更新运动类型检测器的imu信息，如果到达一定的时间间隔，则会检测
|
||||
* 用户的运动类型
|
||||
* Date : 2020/09/02
|
||||
*
|
||||
*---------------------------------------------------------------------**/
|
||||
void updateDetectorImu(imu* imu) {
|
||||
|
||||
buffer_push((BUFFER *)&g_mol_buf[ACCE], (float)FILTER_filter(&g_mol_flt[ACCE], GET_MOL(imu->acc.s)));
|
||||
buffer_push((BUFFER *)&g_mol_buf[GYRO], (float)FILTER_filter(&g_mol_flt[GYRO], GET_MOL(imu->gyr.s)));
|
||||
|
||||
// g_tick_d 实际上是1.28s检测一次，不然检测的频率过高
|
||||
if ((g_detector.tick % (int)(1.28 * IMU_SAMPLING_FREQUENCY)) == 0) {
|
||||
// 检测用户的运动类型
|
||||
int detectResult = pdr_motionTypeDetect();
|
||||
|
||||
//mag_calibration(imu);
|
||||
if (detectResult == DETECTOR_NO_ERROR && g_detector.lastType != g_detector.type) {
|
||||
detectorUpdate(&g_detector);
|
||||
g_detector.lastType = g_detector.type;
|
||||
}
|
||||
}
|
||||
++g_detector.tick;
|
||||
}
|
||||
|
||||
/**---------------------------------------------------------------------
|
||||
* Function : mag_calibration
|
||||
* Description : 收集手机晃动状态下，进行磁力计校准
|
||||
* Date : 2020/09/02 logzhan
|
||||
*---------------------------------------------------------------------**/
|
||||
void mag_calibration(IMU *imu)
|
||||
{
|
||||
if (DETECTOR_TYPE_SWINGING == g_detector.type && CALIBRATE_OFF == mag_calibrate)
|
||||
{
|
||||
mag_buff[mag_count][0] = imu->mag[0];
|
||||
mag_buff[mag_count][1] = imu->mag[1];
|
||||
mag_buff[mag_count][2] = imu->mag[2];
|
||||
mag_count++;
|
||||
}
|
||||
else{
|
||||
mag_count = 0;
|
||||
}
|
||||
if (MAG_BUF_LEN == mag_count && CALIBRATE_OFF == mag_calibrate)
|
||||
{
|
||||
/*处理代码(模拟退火)start*/
|
||||
float xmin = 0.0f;
|
||||
float ymin = 0.0f;
|
||||
float zmin = 0.0f;
|
||||
float xmax = 0.0f;
|
||||
float ymax = 0.0f;
|
||||
float zmax = 0.0f;
|
||||
float err_ori = 0.0;
|
||||
float r[6] = { 0 };
|
||||
float mag_buff_T[3][MAG_BUF_LEN] = { { 0 } };
|
||||
mag_m_trans(mag_buff, mag_buff_T);
|
||||
mag_min(mag_buff_T[0], &xmin);
|
||||
mag_min(mag_buff_T[1], &ymin);
|
||||
mag_min(mag_buff_T[2], &zmin);
|
||||
mag_max(mag_buff_T[0], &xmax);
|
||||
mag_max(mag_buff_T[1], &ymax);
|
||||
mag_max(mag_buff_T[2], &zmax);
|
||||
float xc = 0.5f*(xmax + xmin);
|
||||
float yc = 0.5f*(ymax + ymin);
|
||||
float zc = 0.5f*(zmax + zmin);
|
||||
float a = 0.5f* (float)fabs(xmax - xmin);
|
||||
float b = 0.5f* (float)fabs(ymax - ymin);
|
||||
float c = 0.5f* (float)fabs(zmax - zmin);
|
||||
for (int i = 0; i < MAG_BUF_LEN; i++)
|
||||
{
|
||||
err_ori = err_ori + (float)fabs((mag_buff[i][0] - xc)*(mag_buff[i][0] - xc) / a / a + \
|
||||
(mag_buff[i][1] - yc)*(mag_buff[i][1] - yc) / b / b + \
|
||||
(mag_buff[i][2] - zc)*(mag_buff[i][2] - zc) / c / c - 1);
|
||||
}
|
||||
|
||||
float xclast = xc;
|
||||
float yclast = yc;
|
||||
float zclast = zc;
|
||||
float alast = a;
|
||||
float blast = b;
|
||||
float clast = c;
|
||||
float err_last = err_ori;
|
||||
float xcnew = 0;
|
||||
float ycnew = 0;
|
||||
float zcnew = 0;
|
||||
float anew = 0;
|
||||
float bnew = 0;
|
||||
float cnew = 0;
|
||||
float err_new = 0;
|
||||
for (int i = 0; i < 1000; i++)
|
||||
{
|
||||
mag_rand(r);
|
||||
xcnew = xclast + r[0] - 0.5f;
|
||||
ycnew = yclast + r[1] - 0.5f;
|
||||
zcnew = zclast + r[2] - 0.5f;
|
||||
anew = (float)fabs(alast + r[3] - 0.5);
|
||||
bnew = (float)fabs(blast + r[4] - 0.5);
|
||||
cnew = (float)fabs(clast + r[5] - 0.5);
|
||||
err_new = 0;
|
||||
for (int j = 0; j < MAG_BUF_LEN; j++)
|
||||
{
|
||||
err_new = err_new + (float)fabs((mag_buff[j][0] - xcnew)*(mag_buff[j][0] - xcnew) / anew / anew + \
|
||||
(mag_buff[j][1] - ycnew)*(mag_buff[j][1] - ycnew) / bnew / bnew + \
|
||||
(mag_buff[j][2] - zcnew)*(mag_buff[j][2] - zcnew) / cnew / cnew - 1);
|
||||
}
|
||||
if (err_new < err_ori){
|
||||
xclast = xcnew;
|
||||
yclast = ycnew;
|
||||
zclast = zcnew;
|
||||
alast = anew;
|
||||
blast = bnew;
|
||||
clast = cnew;
|
||||
err_last = err_new;
|
||||
}
|
||||
}
|
||||
float err_0 = 0.0;
|
||||
for (int j = 0; j < MAG_BUF_LEN; j++)
|
||||
{
|
||||
err_0 = err_0 + (float)fabs((mag_buff[j][0])*(mag_buff[j][0]) + (mag_buff[j][1])*(mag_buff[j][1]) + (mag_buff[j][2])*(mag_buff[j][2]) - 1);
|
||||
}
|
||||
/*处理代码(模拟退火)end*/
|
||||
|
||||
if (err_0 > err_last)
|
||||
{
|
||||
ellipsoidPoint[0] = xclast;
|
||||
ellipsoidPoint[1] = yclast;
|
||||
ellipsoidPoint[2] = zclast;
|
||||
ellipsoidScale[0] = alast;
|
||||
ellipsoidScale[1] = blast;
|
||||
ellipsoidScale[2] = clast;
|
||||
mag_calibrate = CALIBRATE_ON;
|
||||
}
|
||||
else
|
||||
{
|
||||
ellipsoidPoint[0] = 0;
|
||||
ellipsoidPoint[1] = 0;
|
||||
ellipsoidPoint[2] = 0;
|
||||
ellipsoidScale[0] = 1;
|
||||
ellipsoidScale[1] = 1;
|
||||
ellipsoidScale[2] = 1;
|
||||
}
|
||||
mag_count = 0;
|
||||
memset(*mag_buff, 0, sizeof(mag_buff));
|
||||
}
|
||||
|
||||
ellipsoidPoint[0] = 0;
|
||||
ellipsoidPoint[1] = 0;
|
||||
ellipsoidPoint[2] = 0;
|
||||
ellipsoidScale[0] = 1;
|
||||
ellipsoidScale[1] = 1;
|
||||
ellipsoidScale[2] = 1;
|
||||
|
||||
imu->mag[0] = (imu->mag[0] - ellipsoidPoint[0]) * ellipsoidScale[0];
|
||||
imu->mag[1] = (imu->mag[1] - ellipsoidPoint[1]) * ellipsoidScale[1];
|
||||
imu->mag[2] = (imu->mag[2] - ellipsoidPoint[2]) * ellipsoidScale[2];
|
||||
}
|
||||
|
||||
/**---------------------------------------------------------------------
|
||||
* Function : mag_rand
|
||||
* Description : 生成固定长度的0-1的随机数数组
|
||||
* Date : 2020/09/02 logzhan
|
||||
*---------------------------------------------------------------------**/
|
||||
void mag_rand(float r[6]){
|
||||
float temp[6] = { 0.0 };
|
||||
srand((unsigned)time(NULL)); //读取系统时间，产生一个种子数值
|
||||
for (int i = 0; i < 6; i++)
|
||||
{
|
||||
temp[i] = rand() / (RAND_MAX + 1.0f);
|
||||
}
|
||||
for (int i = 0; i < 6; i++)
|
||||
{
|
||||
r[i] = temp[i];
|
||||
}
|
||||
}
|
||||
|
||||
/**---------------------------------------------------------------------
|
||||
* Function : mag_min
|
||||
* Description : 返回数组中的最小值
|
||||
* Date : 2020/09/02 logzhan
|
||||
*---------------------------------------------------------------------**/
|
||||
void mag_min(float a[MAG_BUF_LEN],float *mag_min){
|
||||
float temp;
|
||||
temp = a[0];
|
||||
for (int i = 1; i < MAG_BUF_LEN; i++)
|
||||
{
|
||||
if (a[i] < temp)
|
||||
temp = a[i];
|
||||
}
|
||||
*mag_min = temp;
|
||||
}
|
||||
|
||||
/**---------------------------------------------------------------------
|
||||
* Function : mag_max
|
||||
* Description : 返回数组中的最大值
|
||||
* Date : 2020/09/02 logzhan
|
||||
*---------------------------------------------------------------------**/
|
||||
void mag_max(float a[MAG_BUF_LEN], float *mag_max){
|
||||
float temp;
|
||||
temp = a[0];
|
||||
for (int i = 1; i < MAG_BUF_LEN; i++)
|
||||
{
|
||||
if (a[i] > temp)
|
||||
temp = a[i];
|
||||
}
|
||||
*mag_max = temp;
|
||||
}
|
||||
|
||||
/**---------------------------------------------------------------------
|
||||
* Function : mag_m_trans
|
||||
* Description : 矩阵转置
|
||||
* Date : 2020/09/02 logzhan
|
||||
*---------------------------------------------------------------------**/
|
||||
void mag_m_trans(float a[MAG_BUF_LEN][3], float r[3][MAG_BUF_LEN]) {
|
||||
int i, j;
|
||||
|
||||
for (i = 0; i < 3; i++) {
|
||||
for (j = 0; j < MAG_BUF_LEN; j++) {
|
||||
r[i][j] = a[j][i];
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/**---------------------------------------------------------------------
|
||||
* Function : fft_buffer
|
||||
* Description : 对acc数据做傅里叶变换，频率信息保存在fft中
|
||||
* Date : 2020/7/20 logzhan
|
||||
*---------------------------------------------------------------------**/
|
||||
static int fft_buffer(myComplex *fft, BUFFER *buffer) {
|
||||
static float s_signal[1024] = { 0 };
|
||||
|
||||
float mean;
|
||||
int index;
|
||||
int i;
|
||||
|
||||
Buffer_mean(buffer, &mean);
|
||||
for (i = 0, index = buffer->_bottom; index != buffer->_top; ++i, index = INC_PTR(buffer, index)) {
|
||||
s_signal[i] = buffer->data[index] - mean; /*时序上255比0新*/
|
||||
}
|
||||
|
||||
//序号i之后的长度里元素全部置为0
|
||||
memset(s_signal + i, 0, (ARR_LEN(s_signal) - i) * sizeof(*s_signal));
|
||||
|
||||
if (0 == FFT_fft(fft, s_signal, ARR_LEN(s_signal))) {
|
||||
return DETECTOR_OUT_OF_MEMORY;
|
||||
}
|
||||
return DETECTOR_NO_ERROR;
|
||||
}
|
||||
|
||||
/**----------------------------------------------------------------------
|
||||
* Function : find_peaks
|
||||
* Description : 获取前number个较大频点的频率frequency和幅度amplitude
|
||||
* Date : 2020/7/20 logzhan
|
||||
*---------------------------------------------------------------------**/
|
||||
static int find_peaks(float *frequency, float *amplitude, int number, myComplex *fft, int length, float cut_off_frequency, float min_threshold) {
|
||||
float amp_2[2];
|
||||
int min_ind;
|
||||
int dir;
|
||||
int n;
|
||||
int i;
|
||||
int j;
|
||||
|
||||
min_ind = 0;
|
||||
memset(frequency, 0, number * sizeof(*frequency));
|
||||
memset(amplitude, 0, number * sizeof(*amplitude));
|
||||
|
||||
dir = 1;
|
||||
n = (int)(cut_off_frequency * length / IMU_SAMPLING_FREQUENCY);
|
||||
amp_2[1] = fft[0].real * fft[0].real + fft[0].image * fft[0].image;
|
||||
for (i = 1; i < n; ++i) {
|
||||
amp_2[0] = amp_2[1];
|
||||
amp_2[1] = fft[i].real * fft[i].real + fft[i].image * fft[i].image;
|
||||
if (1 == dir) {
|
||||
if (amp_2[1] < amp_2[0]) {
|
||||
if (amp_2[0] > amplitude[min_ind]) {
|
||||
amplitude[min_ind] = amp_2[0];
|
||||
frequency[min_ind] = (float)i;
|
||||
min_ind = 0;
|
||||
for (j = 1; j < number; ++j) {
|
||||
if (amplitude[min_ind] > amplitude[j]) {
|
||||
min_ind = j;
|
||||
}
|
||||
}
|
||||
}
|
||||
dir = -1;
|
||||
}
|
||||
}
|
||||
else {
|
||||
if (amp_2[1] > amp_2[0]) {
|
||||
dir = 1;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
for (i = 0; i < number; ++i) {
|
||||
amplitude[i] = (float)sqrt(amplitude[i]);
|
||||
if (amplitude[i] < min_threshold) {
|
||||
frequency[i] = 0;
|
||||
amplitude[i] = 0;
|
||||
}
|
||||
else {
|
||||
frequency[i] *= (float)IMU_SAMPLING_FREQUENCY / length;
|
||||
}
|
||||
}
|
||||
|
||||
return DETECTOR_NO_ERROR;
|
||||
}
|
||||
|
||||
|
||||
static void get_nonzero_weighted_mean(float *mean, BUFFER_SHORT *buffer, int length) {
|
||||
int weight_sum;
|
||||
int weight;
|
||||
int index;
|
||||
|
||||
if (buffer->_bottom == buffer->_top) {
|
||||
*mean = 0;
|
||||
return;
|
||||
}
|
||||
|
||||
*mean = 0;
|
||||
weight_sum = 0;
|
||||
index = buffer->_top;
|
||||
for (weight = length; weight > 0 && index != buffer->_bottom; --weight) {
|
||||
index = DEC_PTR(buffer, index);
|
||||
if (0 != buffer->data[index]) {
|
||||
*mean += weight * buffer->data[index];
|
||||
weight_sum += weight;
|
||||
}
|
||||
}
|
||||
if (0 != weight_sum) {
|
||||
*mean /= weight_sum;
|
||||
}
|
||||
}
|
||||
|
||||
static void count_zeros(int *count, BUFFER_SHORT *buffer, int length) {
|
||||
int index;
|
||||
|
||||
*count = 0;
|
||||
for (index = (buffer->_top + buffer->length + 1 - length) % (buffer->length + 1); index != buffer->_top; index = INC_PTR(buffer, index)) {
|
||||
if (0 == buffer->data[index]) {
|
||||
++*count;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
static void copy_buffer(BUFFER_SHORT *dst, BUFFER_SHORT *src, int length) {
|
||||
int dst_cnt;
|
||||
int src_cnt;
|
||||
int dst_idx;
|
||||
int src_idx;
|
||||
|
||||
Buffer_count((BUFFER *)dst, &dst_cnt);
|
||||
Buffer_count((BUFFER *)src, &src_cnt);
|
||||
length = length < dst_cnt ? length : dst_cnt;
|
||||
length = length < src_cnt ? length : src_cnt;
|
||||
|
||||
dst_idx = (dst->_top + dst->length + 1 - length) % (dst->length + 1);
|
||||
src_idx = (src->_top + src->length + 1 - length) % (src->length + 1);
|
||||
while (dst_idx != dst->_top) {
|
||||
dst->data[dst_idx] = src->data[src_idx];
|
||||
dst_idx = INC_PTR(dst, dst_idx);
|
||||
src_idx = INC_PTR(src, src_idx);
|
||||
}
|
||||
}
|
||||
|
||||
static void clear_buffer(BUFFER_SHORT *buffer) {
|
||||
int index;
|
||||
|
||||
index = buffer->_bottom;
|
||||
while (index != buffer->_top) {
|
||||
buffer->data[index] = 0;
|
||||
index = INC_PTR(buffer, index);
|
||||
}
|
||||
}
|
||||
|
||||
static int track_frequency(BUFFER_SHORT *frequency_buffer, BUFFER_SHORT *amplitude_buffer, BUFFER_SHORT *temp_buffer,
|
||||
float *frequency, float *amplitude, int number) {
|
||||
static float mean[3];
|
||||
static float cost[3 * 3];
|
||||
static int assignment[3];
|
||||
float distance;
|
||||
float prev;
|
||||
int i;
|
||||
int j;
|
||||
int transform_flag;
|
||||
int count;
|
||||
|
||||
/* 0.detect argument */
|
||||
if (number > ARR_LEN(mean)) {
|
||||
return DETECTOR_OUT_OF_MEMORY;
|
||||
}
|
||||
|
||||
/* 1.caculate main mean */
|
||||
for (i = 0; i < number; ++i) {
|
||||
get_nonzero_weighted_mean(&mean[i], &frequency_buffer[i], TMP_BUF_LEN);
|
||||
}
|
||||
|
||||
/* 2.caculate main cost */
|
||||
for (i = 0; i < number; ++i) {
|
||||
for (j = 0; j < number; ++j) {
|
||||
distance = (float)fabs(mean[i] - frequency[j]);
|
||||
|
||||
if (0 == mean[i] || 0 == frequency[j] || distance > FRQ_RAT) {
|
||||
COST(i, j) = 1e3;
|
||||
}
|
||||
else {
|
||||
COST(i, j) = distance;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/* 3.get main assignment */
|
||||
MUNKRES_get_assignment(assignment, cost, number, number);
|
||||
|
||||
/* 4.push main frequency and main amplitude buffer */
|
||||
for (i = 0; i < number; ++i) {
|
||||
if (1e3 == COST(i, assignment[i])) {
|
||||
buffer_push((BUFFER *)&frequency_buffer[i], 0);
|
||||
buffer_push((BUFFER *)&litude_buffer[i], 0);
|
||||
}
|
||||
else {
|
||||
buffer_push((BUFFER *)&frequency_buffer[i], frequency[assignment[i]]);
|
||||
buffer_push((BUFFER *)&litude_buffer[i], amplitude[assignment[i]]);
|
||||
frequency[assignment[i]] = 0;
|
||||
amplitude[assignment[i]] = 0;
|
||||
}
|
||||
}
|
||||
|
||||
/* 5.caculate temp mean */
|
||||
for (i = 0; i < number; ++i) {
|
||||
get_nonzero_weighted_mean(&mean[i], &temp_buffer[i], TMP_BUF_LEN);
|
||||
}
|
||||
|
||||
/* 6.caculate temp cost */
|
||||
for (i = 0; i < number; ++i) {
|
||||
for (j = 0; j < number; ++j) {
|
||||
if (0 == frequency[j]) {
|
||||
COST(i, j) = 1e3;
|
||||
}
|
||||
else if (0 == mean[i]) {
|
||||
COST(i, j) = FRQ_RAT;
|
||||
}
|
||||
else {
|
||||
COST(i, j) = (float)fabs(mean[i] - frequency[j]);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/* 7.get main assignment */
|
||||
MUNKRES_get_assignment(assignment, cost, number, number);
|
||||
|
||||
/* 8.push temp buffer */
|
||||
for (i = 0; i < number; ++i) {
|
||||
buffer_push((BUFFER *)&temp_buffer[i], frequency[assignment[i]]);
|
||||
}
|
||||
|
||||
/* 9.caculate temp mean */
|
||||
for (i = 0; i < number; ++i) {
|
||||
get_nonzero_weighted_mean(&mean[i], &temp_buffer[i], TMP_BUF_LEN);
|
||||
}
|
||||
|
||||
/* 10.check temp buffer */
|
||||
transform_flag = 0;
|
||||
for (i = 0; i < number; ++i) {
|
||||
|
||||
prev = temp_buffer[i].data[temp_buffer[i]._bottom];
|
||||
for (j = temp_buffer[i]._bottom; j != temp_buffer[i]._top; j = INC_PTR(&temp_buffer[i], j)) {
|
||||
if (0 == temp_buffer[i].data[j] || fabs(temp_buffer[i].data[j] - prev) > FRQ_RAT) {
|
||||
break;
|
||||
}
|
||||
prev = temp_buffer[i].data[j];
|
||||
}
|
||||
if (j == temp_buffer[i]._top) {
|
||||
frequency[i] = mean[i];
|
||||
transform_flag = 1;
|
||||
}
|
||||
else {
|
||||
frequency[i] = 0;
|
||||
}
|
||||
}
|
||||
|
||||
if (transform_flag) {
|
||||
/* 11.caculate main mean */
|
||||
for (i = 0; i < number; ++i) {
|
||||
get_nonzero_weighted_mean(&mean[i], &frequency_buffer[i], TMP_BUF_LEN);
|
||||
}
|
||||
|
||||
/* 12.caculate main cost */
|
||||
for (i = 0; i < number; ++i) {
|
||||
|
||||
count_zeros(&count, &frequency_buffer[i], TMP_BUF_LEN);
|
||||
for (j = 0; j < number; ++j) {
|
||||
if (0 == frequency[j]) {
|
||||
COST(i, j) = 1e3;
|
||||
}
|
||||
else if (0 == mean[i]) {
|
||||
COST(i, j) = -5;
|
||||
}
|
||||
else if (count >= 3) {
|
||||
// COST(i, j) = 3 - count;
|
||||
COST(i, j) = (float)(-5 + fabs(mean[i] - frequency[j]));
|
||||
}
|
||||
else {
|
||||
COST(i, j) = (float)fabs(mean[i] - frequency[j]);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/* 13.get main assignment */
|
||||
MUNKRES_get_assignment(assignment, cost, number, number);
|
||||
|
||||
/* 14.push main frequency and main amplitude buffer */
|
||||
for (i = 0; i < number; ++i) {
|
||||
if (0 != frequency[assignment[i]]) {
|
||||
// Buffer_push(&frequency_buffer[i], frequency[assignment[i]]);
|
||||
// Buffer_push(&litude_buffer[i], 0);
|
||||
copy_buffer(&frequency_buffer[i], &temp_buffer[assignment[i]], TMP_BUF_LEN);
|
||||
clear_buffer(&temp_buffer[assignment[i]]);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
return DETECTOR_NO_ERROR;
|
||||
}
|
||||
|
||||
static void get_nonzero_mean(float *mean, BUFFER_SHORT *buffer, int length) {
|
||||
int count;
|
||||
int index;
|
||||
|
||||
count = (buffer->_top - buffer->_bottom + buffer->length + 1) % (buffer->length + 1);
|
||||
length = count < length ? count : length;
|
||||
*mean = 0;
|
||||
count = 0;
|
||||
for (index = (buffer->_top + buffer->length + 1 - length) % (buffer->length + 1); index != buffer->_top; index = INC_PTR(buffer, index)) {
|
||||
if (0 != buffer->data[index]) {
|
||||
*mean += buffer->data[index];
|
||||
++count;
|
||||
}
|
||||
}
|
||||
if (count >= 0.5 * length) {
|
||||
*mean /= count;
|
||||
}
|
||||
}
|
||||
|
||||
static void get_nonzero_std(float *std, BUFFER_SHORT *buffer, int length) {
|
||||
float mean;
|
||||
float delta;
|
||||
int count;
|
||||
int index;
|
||||
|
||||
count = (buffer->_top - buffer->_bottom + buffer->length + 1) % (buffer->length + 1);
|
||||
length = count < length ? count : length;
|
||||
mean = 0;
|
||||
count = 0;
|
||||
for (index = (buffer->_top + buffer->length + 1 - length) % (buffer->length + 1); index != buffer->_top; index = INC_PTR(buffer, index)) {
|
||||
if (0 != buffer->data[index]) {
|
||||
mean += buffer->data[index];
|
||||
++count;
|
||||
}
|
||||
}
|
||||
if (0 != count) {
|
||||
mean /= count;
|
||||
}
|
||||
|
||||
*std = 0;
|
||||
for (index = (buffer->_top + buffer->length + 1 - length) % (buffer->length + 1); index != buffer->_top; index = INC_PTR(buffer, index)) {
|
||||
if (0 != buffer->data[index]) {
|
||||
delta = buffer->data[index] - mean;
|
||||
*std += delta * delta;
|
||||
}
|
||||
}
|
||||
if (count >= 0.5 * length) {
|
||||
*std = (float)sqrt(*std / count);
|
||||
}
|
||||
else {
|
||||
*std = -1; // default;
|
||||
}
|
||||
}
|
||||
|
||||
/**---------------------------------------------------------------------
|
||||
* Function : get_feature
|
||||
* Description : 获取用于判断手机携带方式（静止、手持、摆手等）的特征
|
||||
* Date : 2020/7/20 logzhan
|
||||
*---------------------------------------------------------------------**/
|
||||
static void get_feature(float *feature, int length) {
|
||||
int i;
|
||||
int j;
|
||||
float temp;
|
||||
|
||||
Buffer_mean((BUFFER *)&g_mol_buf[ACCE], &feature[0]);
|
||||
Buffer_std( (BUFFER *)&g_mol_buf[ACCE], &feature[1]);
|
||||
Buffer_std( (BUFFER *)&g_mol_buf[GYRO], &feature[2]);
|
||||
get_nonzero_std( &feature[3] , &g_acc_frq_buf[0], length); if (feature[3] < 0) feature[3] = 10;
|
||||
get_nonzero_std( &feature[4] , &g_acc_frq_buf[1], length); if (feature[4] < 0) feature[4] = 10;
|
||||
get_nonzero_std( &feature[5] , &g_acc_frq_buf[2], length); if (feature[5] < 0) feature[5] = 10;
|
||||
get_nonzero_mean(&feature[6] , &g_acc_amp_buf[0], length);
|
||||
get_nonzero_mean(&feature[7] , &g_acc_amp_buf[1], length);
|
||||
get_nonzero_mean(&feature[8] , &g_acc_amp_buf[2], length);
|
||||
get_nonzero_std( &feature[9] , &g_gyr_frq_buf[0], length); if (feature[9] < 0) feature[9] = 10;
|
||||
get_nonzero_std( &feature[10], &g_gyr_frq_buf[1], length); if (feature[10] < 0) feature[10] = 10;
|
||||
get_nonzero_std( &feature[11], &g_gyr_frq_buf[2], length); if (feature[11] < 0) feature[11] = 10;
|
||||
get_nonzero_mean(&feature[12], &g_gyr_amp_buf[0], length);
|
||||
get_nonzero_mean(&feature[13], &g_gyr_amp_buf[1], length);
|
||||
get_nonzero_mean(&feature[14], &g_gyr_amp_buf[2], length);
|
||||
|
||||
/* sort by std - bubble sort */
|
||||
for (i = 0; i < 2; ++i) {
|
||||
for (j = 0; j < 2 - i; ++j) {
|
||||
if (feature[3 + j] > feature[3 + j + 1]) {
|
||||
|
||||
temp = feature[3 + j];
|
||||
feature[3 + j] = feature[3 + j + 1];
|
||||
feature[3 + j + 1] = temp;
|
||||
temp = feature[6 + j];
|
||||
feature[6 + j] = feature[6 + j + 1];
|
||||
feature[6 + j + 1] = temp;
|
||||
}
|
||||
}
|
||||
}
|
||||
for (i = 0; i < 2; ++i) {
|
||||
for (j = 0; j < 2 - i; ++j) {
|
||||
if (feature[9 + j] > feature[9 + j + 1]) {
|
||||
|
||||
temp = feature[9 + j];
|
||||
feature[9 + j] = feature[9 + j + 1];
|
||||
feature[9 + j + 1] = temp;
|
||||
temp = feature[12 + j];
|
||||
feature[12 + j] = feature[12 + j + 1];
|
||||
feature[12 + j + 1] = temp;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/**---------------------------------------------------------------------
|
||||
* Function : predict
|
||||
* Description : 判断手机携带方式（静止、手持、摆手等）
|
||||
* Date : 2020/7/20 logzhan
|
||||
*---------------------------------------------------------------------**/
|
||||
int pdr_detectorPredict(float *feature)
|
||||
{
|
||||
if (feature[1] < 0.932921) {
|
||||
return DETECTOR_TYPE_STATIC;
|
||||
}else if ((feature[3] < 0.10) + (feature[4] < 0.10) + (feature[5] < 0.10) > 0) {
|
||||
if (feature[12] < 49.9795) {
|
||||
if (feature[0] < 10.7220) {
|
||||
return DETECTOR_TYPE_HANDHELD;
|
||||
}
|
||||
else {
|
||||
return DETECTOR_TYPE_SWINGING;
|
||||
}
|
||||
}
|
||||
else {
|
||||
if (feature[2] < 0.312002) {
|
||||
return DETECTOR_TYPE_HANDHELD;
|
||||
}
|
||||
else {
|
||||
return DETECTOR_TYPE_SWINGING;
|
||||
}
|
||||
}
|
||||
}
|
||||
return DETECTOR_TYPE_IRREGULAR;
|
||||
}
|
||||
|
||||
/**---------------------------------------------------------------------
|
||||
* Function : debounce
|
||||
* Description : 去抖动，判断之前length个label，根据较多的携带方式最终确定手
|
||||
机携带方式
|
||||
* Date : 2020/7/20 logzhan
|
||||
*---------------------------------------------------------------------**/
|
||||
static int debounce(int *label, int length) {
|
||||
int cnt[4];
|
||||
int def;
|
||||
int i;
|
||||
|
||||
memset(cnt, 0, sizeof(cnt));
|
||||
for (i = 0; i < length; ++i) {
|
||||
++cnt[label[i]];
|
||||
}
|
||||
|
||||
def = 0;
|
||||
for (i = 0; i < ARR_LEN(cnt); ++i) {
|
||||
if (cnt[i] > length * 3 / 4) {
|
||||
return i;
|
||||
}
|
||||
if (cnt[i] > 0) {
|
||||
def = i;
|
||||
}
|
||||
}
|
||||
return def;
|
||||
}
|
||||
|
||||
/**---------------------------------------------------------------------
|
||||
* Function : pdr_motionTypeDetect
|
||||
* Description : pdr运动类型检测器
|
||||
* Date : 2020/7/20 logzhan
|
||||
*---------------------------------------------------------------------**/
|
||||
int pdr_motionTypeDetect() {
|
||||
static myComplex s_fft[1024] = { {0.0} };
|
||||
static float s_feature[15] = { 0 };
|
||||
float frq[3];
|
||||
float amp[3];
|
||||
int ret;
|
||||
int i;
|
||||
|
||||
// 分析加速度计
|
||||
if (DETECTOR_NO_ERROR != (ret = fft_buffer(s_fft, (BUFFER *)&g_mol_buf[ACCE]))) {
|
||||
return ret;
|
||||
}
|
||||
if (DETECTOR_NO_ERROR != (ret = find_peaks(frq, amp, 3, s_fft, 1024, 5, 0))) {
|
||||
return ret;
|
||||
}
|
||||
if (DETECTOR_NO_ERROR != (ret = track_frequency(g_acc_frq_buf, g_acc_amp_buf, g_acc_tmp_buf, frq, amp, 3))) {
|
||||
return ret;
|
||||
}
|
||||
// 分析陀螺仪
|
||||
if (DETECTOR_NO_ERROR != (ret = fft_buffer(s_fft, (BUFFER *)&g_mol_buf[GYRO]))) {
|
||||
return ret;
|
||||
}
|
||||
if (DETECTOR_NO_ERROR != (ret = find_peaks(frq, amp, 3, s_fft, 1024, 5, (DETECTOR_TYPE_SWINGING == g_detector.type ||
|
||||
DETECTOR_TYPE_HANDHELD == g_detector.type) ? 20.0f : 100.0f))) {
|
||||
return ret;
|
||||
}
|
||||
if (DETECTOR_NO_ERROR != (ret = track_frequency(g_gyr_frq_buf, g_gyr_amp_buf, g_gyr_tmp_buf, frq, amp, 3))) {
|
||||
return ret;
|
||||
}
|
||||
|
||||
get_feature(s_feature, 5);
|
||||
|
||||
g_detector.type = pdr_detectorPredict(s_feature);
|
||||
|
||||
/* debounce */
|
||||
for (i = 0; i < ARR_LEN(g_label) - 1; ++i) {
|
||||
g_label[i] = g_label[i + 1];
|
||||
}
|
||||
g_label[i] = g_detector.type;
|
||||
g_detector.type = debounce(g_label, ARR_LEN(g_label));
|
||||
|
||||
return DETECTOR_NO_ERROR;
|
||||
}
|
||||
|
|
@ -1,180 +0,0 @@
|
|||
/*****************************************************************
|
||||
*Module Name: filter
|
||||
*Module Date: 2018.10.24
|
||||
*Module Auth: chelimin11070377
|
||||
*****************************************************************/
|
||||
|
||||
/* Header File Including ------------------------------------------------------------------------ */
|
||||
#include "stdio.h"
|
||||
#include "Filter.h"
|
||||
|
||||
/* Function Definition -------------------------------------------------------------------------- */
|
||||
|
||||
/*****************************************************************
|
||||
*Description: reset filter
|
||||
*Input : *f filter
|
||||
*Output: none
|
||||
*Return: none
|
||||
*****************************************************************/
|
||||
void FILTER_reset(FILTER *f) {
|
||||
f->reset = 1;
|
||||
}
|
||||
|
||||
/*****************************************************************
|
||||
*Description: reset fir filter
|
||||
*Input : *f fir filter
|
||||
*Output: none
|
||||
*Return: none
|
||||
*****************************************************************/
|
||||
void FILTER_FIR_reset(FILTER_FIR *f) {
|
||||
f->reset = 1;
|
||||
}
|
||||
|
||||
/*****************************************************************
|
||||
*Description: set filter coefficient
|
||||
*Input : order coefficient array length, not filter order
|
||||
* *b b coefficient
|
||||
* *a a coefficient
|
||||
* H0 dc signal gain
|
||||
*Output: *f filter
|
||||
*Return: none
|
||||
*****************************************************************/
|
||||
void FILTER_setCoef(FILTER *f, int order, double *b, double *a, double H0) {
|
||||
int i;
|
||||
|
||||
for (i = 0; i < order; ++i) {
|
||||
f->b[i] = b[i];
|
||||
f->a[i] = a[i];
|
||||
}
|
||||
f->order = order;
|
||||
f->H0 = H0;
|
||||
f->reset = 1;
|
||||
}
|
||||
|
||||
/*****************************************************************
|
||||
*Description: set fir filter coefficient
|
||||
*Input : order coefficient array length, not filter order
|
||||
* *b b coefficient
|
||||
* *a a coefficient
|
||||
* H0 dc signal gain
|
||||
*Output: *f fir filter
|
||||
*Return: none
|
||||
*****************************************************************/
|
||||
void FILTER_FIR_setCoef(FILTER_FIR *f, int order, double *b, double H0) {
|
||||
int i;
|
||||
|
||||
for (i = 0; i < order; ++i) {
|
||||
f->b[i] = b[i];
|
||||
}
|
||||
f->order = order;
|
||||
f->H0 = H0;
|
||||
f->reset = 1;
|
||||
}
|
||||
|
||||
/*****************************************************************
|
||||
*Description: filter x signal
|
||||
*Input : *f filter
|
||||
* x signal
|
||||
*Output: none
|
||||
*Return: filter result
|
||||
*****************************************************************/
|
||||
double FILTER_filter(FILTER *f, double x) {
|
||||
int i;
|
||||
double y;
|
||||
|
||||
if (f->order < 1) {
|
||||
printf("fileter order is set to incorrect order, pls chk \
|
||||
your code!\n");
|
||||
return 0;
|
||||
}
|
||||
|
||||
if (f->reset) {
|
||||
for (i = 0; i < f->order; ++i) {
|
||||
f->yout[i] = f->H0 * x;
|
||||
f->xin[i] = x;
|
||||
}
|
||||
y = f->yout[f->order - 1];
|
||||
f->reset = 0;
|
||||
}
|
||||
// push x and y
|
||||
for (i = 0; i < f->order - 1; ++i) {
|
||||
f->xin[i] = f->xin[i + 1];
|
||||
f->yout[i] = f->yout[i + 1];
|
||||
}
|
||||
f->xin[f->order - 1] = x;
|
||||
// filter
|
||||
y = f->b[0] * f->xin[f->order - 1];
|
||||
for (i = 1; i < f->order; ++i) {
|
||||
y += f->b[i] * f->xin[f->order - 1 - i] - f->a[i] * f->yout[f->order - 1 - i];
|
||||
}
|
||||
f->yout[f->order - 1] = y;
|
||||
|
||||
return y / f->H0;
|
||||
}
|
||||
|
||||
/*****************************************************************
|
||||
*Description: filter x signal
|
||||
*Input : *f fir filter
|
||||
* x signal
|
||||
*Output: none
|
||||
*Return: filter result
|
||||
*****************************************************************/
|
||||
double FILTER_FIR_filter(FILTER_FIR *f, double x) {
|
||||
int i;
|
||||
double y;
|
||||
|
||||
if (f->order < 1) {
|
||||
printf("fileter order is set to incorrect order, pls chk \
|
||||
your code!\n");
|
||||
return 0;
|
||||
}
|
||||
|
||||
if (f->reset) {
|
||||
for (i = 0; i < f->order; ++i) {
|
||||
f->xin[i] = x;
|
||||
}
|
||||
f->reset = 0;
|
||||
}
|
||||
// push x
|
||||
for (i = 0; i < f->order - 1; ++i) {
|
||||
f->xin[i] = f->xin[i + 1];
|
||||
}
|
||||
f->xin[f->order - 1] = x;
|
||||
// filter
|
||||
y = f->b[0] * f->xin[f->order - 1];
|
||||
for (i = 1; i < f->order; ++i) {
|
||||
y += f->b[i] * f->xin[f->order - 1 - i];
|
||||
}
|
||||
|
||||
return y / f->H0;
|
||||
}
|
||||
|
||||
void DELAY_reset(DELAYER *d) {
|
||||
d->reset = 1;
|
||||
}
|
||||
|
||||
void DELAY_setCoef(DELAYER *d, int delay) {
|
||||
d->delay = delay;
|
||||
d->reset = 1;
|
||||
}
|
||||
|
||||
double DELAY_delay(DELAYER *d, double x) {
|
||||
double yout = d->xin[0];
|
||||
int i;
|
||||
|
||||
if (d->reset) {
|
||||
for (i = 0; i < d->delay; ++i) {
|
||||
d->xin[i] = x;
|
||||
}
|
||||
yout = x;
|
||||
d->reset = 0;
|
||||
}
|
||||
else {
|
||||
for (i = 0; i < d->delay - 1; ++i) {
|
||||
d->xin[i] = d->xin[i + 1];
|
||||
}
|
||||
d->xin[d->delay - 1] = x;
|
||||
}
|
||||
|
||||
return yout;
|
||||
}
|
||||
|
|
@ -1,890 +0,0 @@
|
|||
/******************** (C) COPYRIGHT 2020 Geek************************************
|
||||
* File Name : pdr_kalman.c
|
||||
* Current Version : V2.0(compare QCOM SAP 5.0)
|
||||
* Author : logzhan
|
||||
* Date of Issued : 2020.7.3
|
||||
* Comments : PDR 卡尔曼滤波器相关
|
||||
********************************************************************************/
|
||||
#include <stdio.h>
|
||||
#include <string.h>
|
||||
#include <math.h>
|
||||
#if !defined(LOCATION_PLATFORM_QCOM_MODEM)
|
||||
#include <malloc.h>
|
||||
#endif
|
||||
#include "pdr_base.h"
|
||||
#include "pdr_kalman.h"
|
||||
#include "pdr_location.h"
|
||||
#include "pdr_matrix.h"
|
||||
#include "pdr_util.h"
|
||||
#include "pdr_linearFit.h"
|
||||
|
||||
#if USE_BUG_FOR_LOCAL_PARA
|
||||
static double Ekf_Upd_buf_2d[5][N] = { {0.0} };
|
||||
static double Ekf_Upd_buf_3d[7][N][N] = { { {0.0} } };
|
||||
#endif
|
||||
|
||||
extern double GpsPos[HIST_GPS_NUM][3];
|
||||
|
||||
extern int debugCount;
|
||||
extern double angle_mean[3];
|
||||
|
||||
static int hold_type;
|
||||
static double b_timestamp;
|
||||
static double b_last_time;
|
||||
static double attitude_yaw;
|
||||
static DETECTOR *g_detector;
|
||||
static int hold_type;
|
||||
static double BUL_duration;
|
||||
static double FUR_duration;
|
||||
|
||||
/**----------------------------------------------------------------------
|
||||
* Function : pdr_initKalman
|
||||
* Description : 初始化卡尔曼滤波器相关
|
||||
* Date : 2020/8/27 logzhan
|
||||
*---------------------------------------------------------------------**/
|
||||
void pdr_initKalman(void)
|
||||
{
|
||||
b_timestamp = -1;
|
||||
b_last_time = -1;
|
||||
BUL_duration = -1;
|
||||
FUR_duration = -1;
|
||||
attitude_yaw = -1;
|
||||
hold_type = 0;
|
||||
}
|
||||
|
||||
int clear_buf(double *buf_p, int dmt)
|
||||
{
|
||||
int bufLen = 0;
|
||||
double *buf_pdr = (double *)buf_p;
|
||||
|
||||
if (NULL == buf_p)return -1;
|
||||
|
||||
if (!(2 == dmt || 1 == dmt)){
|
||||
return -1;
|
||||
}
|
||||
else if (2 == dmt)bufLen = N * N;
|
||||
else if (1 == dmt)bufLen = N;
|
||||
|
||||
for (int i = 0; i < bufLen; i++){
|
||||
buf_pdr[i] = 0.0f;
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
|
||||
/**----------------------------------------------------------------------
|
||||
* Function : pdr_ekfStatePredict
|
||||
* Description : pdr卡尔曼滤波器的状态预测方程
|
||||
* Date : 2020/7/22 logzhan
|
||||
*---------------------------------------------------------------------**/
|
||||
void pdr_ekfStatePredict(KfPara *kf, double stepLen, PDR* g_pdr, int step) {
|
||||
|
||||
double (*phi)[N] = (double(*)[N])&(Ekf_Upd_buf_3d[0][0][0]);
|
||||
double (*pvec1)[N] = (double(*)[N])&(Ekf_Upd_buf_3d[1][0][0]);
|
||||
double (*pvec2)[N] = (double(*)[N])&(Ekf_Upd_buf_3d[2][0][0]);
|
||||
|
||||
clear_buf((double *)phi, BUF_DMT_1);
|
||||
clear_buf((double *)pvec1, BUF_DMT_1);
|
||||
clear_buf((double *)pvec2, BUF_DMT_1);
|
||||
|
||||
double deltaHeading = g_pdr->heading - g_pdr->lastHeading;
|
||||
double angle = g_pdr->heading;
|
||||
|
||||
//printf("deltaHeading = %f\n", R2D(deltaHeading));
|
||||
|
||||
//if (fabs(deltaHeading) < D2R(10) && g_pdr->lastHeading != 0.0f) {
|
||||
// angle = deltaHeading + kf->p_xk[3];
|
||||
//}else {
|
||||
// angle = g_pdr->heading;
|
||||
//}
|
||||
|
||||
if (g_pdr->motionType != PDR_MOTION_TYPE_HANDHELD) {
|
||||
deltaHeading = 0;
|
||||
}
|
||||
|
||||
angle = deltaHeading + kf->p_xk[3];
|
||||
|
||||
|
||||
if (fabs(deltaHeading) > 5.5) {
|
||||
//printf("%f %f\n", R2D(g_pdr->heading), R2D(g_pdr->lastHeading));
|
||||
//printf("d heading = %f\n", R2D(deltaHeading));
|
||||
if (deltaHeading < 0) {
|
||||
deltaHeading = deltaHeading + D2R(360);
|
||||
}else {
|
||||
deltaHeading = deltaHeading - D2R(360);
|
||||
}
|
||||
deltaHeading = 0;
|
||||
angle = deltaHeading + kf->p_xk[3];
|
||||
}
|
||||
|
||||
if(step == 0){
|
||||
kf->p_xk[3] = angle;
|
||||
}
|
||||
|
||||
|
||||
|
||||
// xk+1 = xk + step * cos(angle) PDR位置更新方程
|
||||
// yk+1 = yk + step * sin(angle) PDR位置更新方程
|
||||
// sk+1 = sk 步长和上一次相同
|
||||
// ak+1 = ak * (angle / ak) 或 ak+1 = ak
|
||||
phi[0][0] = 1; phi[1][0] = 0; phi[2][0] = 0; phi[3][0] = 0;
|
||||
phi[0][1] = 0; phi[1][1] = 1; phi[2][1] = 0; phi[3][1] = 0;
|
||||
phi[0][2] = cos(angle); phi[1][2] = sin(angle); phi[2][2] = 1; phi[3][2] = 0;
|
||||
phi[0][3] = 0; phi[1][3] = 0; phi[2][3] = 0; phi[3][3] = 1;
|
||||
|
||||
|
||||
for (uchar i = 0; i < step; i++) {
|
||||
// logzhan 21/02/06:
|
||||
// phi[3][3]是为让kf的预测每次都是最新更新值, 如果phi[3][3]每次都是1，可能在惯性
|
||||
// 导航更新过程中，偏航角的预测是固定的，实时性没有那么好，下面if判断可以在一定程度
|
||||
// 让预测的值更新，但是效果暂时没有感觉出明显的好
|
||||
if (fabs(kf->p_xk[3]) > 0.000001 &&
|
||||
fabs(angle / kf->p_xk[3]) < 3)
|
||||
{
|
||||
phi[3][3] = angle / kf->p_xk[3];
|
||||
}
|
||||
|
||||
// 卡尔曼状态更新方程
|
||||
// p_xk = phi * p_xk
|
||||
VecMatMultiply(kf->p_xk, phi, kf->p_xk);
|
||||
|
||||
// 卡尔曼更新协方差矩阵 : Pk = FPFt + Q
|
||||
MatrixMultiply(phi, kf->p_pk, pvec1); // F*P
|
||||
MatrixTrans(phi, pvec2); // Ft
|
||||
MatrixMultiply(pvec1, pvec2, pvec1); // F*P*Ft
|
||||
MatrixAdd(pvec1, kf->q, kf->p_pk); // F*P*Ft + Q
|
||||
}
|
||||
}
|
||||
|
||||
/**----------------------------------------------------------------------
|
||||
* Function : pdr_ekfStateUpdate
|
||||
* Description : pdr卡尔曼滤波器的状态更新方程
|
||||
* Date : 2020/7/22 logzhan
|
||||
*---------------------------------------------------------------------**/
|
||||
void pdr_ekfStateUpdate(KfPara *kf, gnss *pgnss, classifer *sys, PDR *g_pdr,
|
||||
int scene_type) {
|
||||
|
||||
int j;
|
||||
|
||||
double(*H)[N] = (double(*)[N])&(Ekf_Upd_buf_3d[0][0][0]);
|
||||
double *z = &(Ekf_Upd_buf_2d[0][0]);
|
||||
double(*I)[N] = (double(*)[N])&(Ekf_Upd_buf_3d[1][0][0]);
|
||||
double(*pvec1)[N] = (double(*)[N])&(Ekf_Upd_buf_3d[2][0][0]);
|
||||
double(*Ht)[N] = (double(*)[N])&(Ekf_Upd_buf_3d[3][0][0]);
|
||||
double(*pvec3)[N] = (double(*)[N])&(Ekf_Upd_buf_3d[4][0][0]);
|
||||
double(*pvec4)[N] = (double(*)[N])&(Ekf_Upd_buf_3d[5][0][0]);
|
||||
double(*pvec5) = &(Ekf_Upd_buf_2d[1][0]);
|
||||
double(*pvec6) = &(Ekf_Upd_buf_2d[2][0]);
|
||||
|
||||
double lambda = 0;
|
||||
double(*pv1) = &(Ekf_Upd_buf_2d[3][0]);
|
||||
double(*pv2) = &(Ekf_Upd_buf_2d[4][0]);
|
||||
double(*pv3)[N] = (double(*)[N])&(Ekf_Upd_buf_3d[6][0][0]);
|
||||
|
||||
clear_buf((double *)z, BUF_DMT_1);
|
||||
clear_buf((double *)pvec5, BUF_DMT_1);
|
||||
clear_buf((double *)pvec6, BUF_DMT_1);
|
||||
clear_buf((double *)pv1, BUF_DMT_1);
|
||||
clear_buf((double *)pv2, BUF_DMT_1);
|
||||
|
||||
clear_buf((double *)H, BUF_DMT_2);
|
||||
clear_buf((double *)I, BUF_DMT_2);
|
||||
clear_buf((double *)pvec1, BUF_DMT_2);
|
||||
clear_buf((double *)Ht, BUF_DMT_2);
|
||||
clear_buf((double *)pvec3, BUF_DMT_2);
|
||||
clear_buf((double *)pvec4, BUF_DMT_2);
|
||||
clear_buf((double *)pv3, BUF_DMT_2);
|
||||
|
||||
|
||||
for (char i = 0; i < 4; i++) {
|
||||
H[i][i] = 1;
|
||||
I[i][i] = 1;
|
||||
}
|
||||
|
||||
// 获取观测向量
|
||||
z[0] = pgnss->xNed; // 北向位置
|
||||
z[1] = pgnss->yNed; // 东向位置
|
||||
z[2] = 0.5; // 步长默认0.5
|
||||
|
||||
// 这个if条件实际是使用GPS的位置变化量和步数变化量估计步长, 这部分代码还没
|
||||
// 调好，实际类似于步长不变
|
||||
//if (pgnss->error > 0 && pgnss->error < 3.8 && pgnss->HDOP <= 0.4 &&
|
||||
// g_pdr->deltaStepsPerGps != 0 && g_pdr->deltaStepsPerGps < 5) {
|
||||
// // 利用GPS速度和步频估计步长
|
||||
// //if (g_pdr->motionFreq > 2.5 && (sys->type == 1)) {
|
||||
// //
|
||||
// // //printf("z[2] = %f\n", z[2]);
|
||||
// //}
|
||||
// z[2] = pgnss->vel * 0.514 / (g_pdr->deltaStepsPerGps);
|
||||
//}else {
|
||||
// z[2] = kf->p_xk[2];
|
||||
//}
|
||||
|
||||
z[3] = pgnss->yaw * PI / 180; // 观测的航向角为GPS的航向角
|
||||
|
||||
// 计算 K = K = PHt / (R + H*P*H^T)
|
||||
MatrixTrans(H, Ht); // 计算Ht
|
||||
MatrixMultiply(H, kf->p_pk, pvec1); // 计算HP
|
||||
MatrixMultiply(pvec1, Ht, pvec3); // pvec3 = H*P*H^T
|
||||
MatrixAdd(pvec3, kf->r, pvec4); // pvec4 = R + H*P*H^T
|
||||
MatrixInverse(pvec4, pvec1); // 计算 1 / (R + H*P*H^T)
|
||||
|
||||
for (char i = 0; i < N; i++) {
|
||||
for (uchar j = 0; j < N; j++) {
|
||||
pv3[i][j] = pvec3[i][j];
|
||||
}
|
||||
}
|
||||
MatrixMultiply(kf->p_pk, Ht, pvec3); // 计算PHt
|
||||
MatrixMultiply(pvec3, pvec1, kf->Kk); // 计算增益K = PHt / (R + H*P*H^T)
|
||||
|
||||
|
||||
VecMatMultiply(kf->p_xk, H, pvec5); // pvec5 = Hx'
|
||||
VectorSub(z, pvec5, pvec6); // pvec6 = Z - Hx'
|
||||
modAngle(&pvec6[3], -PI, PI);
|
||||
for (char i = 0; i < N; i++) {
|
||||
pv1[i] = pvec6[i];
|
||||
}
|
||||
|
||||
VecMatMultiply(pvec6, kf->Kk, pvec5); // pvec5 = K*( z - Hx')
|
||||
// Get the optimally updated state estimation
|
||||
VectorAdd(kf->p_xk, pvec5, kf->xk);
|
||||
|
||||
// ---- prepare the matrix for updated estimate covariance ------
|
||||
// pvec1 = K*H
|
||||
MatrixMultiply(kf->Kk, H, pvec1);
|
||||
// pvec2 = (I - K*H)
|
||||
MatrixSub(I, pvec1, Ht);
|
||||
// pvec3 = (I - K*H)*P
|
||||
MatrixMultiply(Ht, kf->p_pk, pvec3);
|
||||
// pvec4 = (I- K*H)^T
|
||||
MatrixTrans(Ht, pvec4);
|
||||
// pvec1 = (I - K*H)*P*(I- K*H)^T
|
||||
MatrixMultiply(pvec3, pvec4, pvec1);
|
||||
|
||||
// pvec2 = K*R
|
||||
MatrixMultiply(kf->Kk, kf->r, Ht);
|
||||
// pvec3 = K^T
|
||||
MatrixTrans(kf->Kk, pvec3);
|
||||
// pvec4 = K*R*K^T
|
||||
MatrixMultiply(Ht, pvec3, pvec4);
|
||||
// Get the updated estimate covariance: P' = (I - K*H)*P*(I- K*H)^T + K*R*K^T
|
||||
MatrixAdd(pvec1, pvec4, kf->pk);
|
||||
|
||||
// pv2 = (z - Hx')*( H*P*H^T )
|
||||
VecMatMultiply(pv1, pv3, pv2);
|
||||
// inner product the pre-fit residual and the transitioned residual: lamda = (z - Hx')*( H*P*H^T ) .* (z-Hx')
|
||||
// Calculate the difference between original differenc and the transitioned,
|
||||
// if the value is close to zero, it shows that there is a big gap in the
|
||||
// oprimization. Thus, we believe our PDR algorithm
|
||||
for (char i = 0; i < N; i++) {
|
||||
lambda += pv2[i] * pv1[i];
|
||||
}
|
||||
|
||||
kf->lambda = lambda;
|
||||
kf->plat = pgnss->lat;
|
||||
kf->plon = pgnss->lon;
|
||||
|
||||
if (lambda >= 200)return;
|
||||
|
||||
for (char i = 0; i < N; i++) {
|
||||
kf->xk[i] = kf->p_xk[i];
|
||||
for (j = 0; j < N; j++) {
|
||||
kf->pk[i][j] = kf->p_pk[i][j];
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
void calStepLastTime(StepPredict *stepPredict, double timestamp, unsigned long steps_last,
|
||||
unsigned long steps)
|
||||
{
|
||||
float tmpTime = 0;
|
||||
if (stepPredict->lastTime > 0 && stepPredict->deltaStep == 0 && (steps - steps_last) > 0)
|
||||
{
|
||||
tmpTime = (float)((timestamp - stepPredict->lastTime) / (steps - steps_last));
|
||||
if (tmpTime > 300 && tmpTime < 800)
|
||||
{
|
||||
if (stepPredict->meanTime > 0){
|
||||
stepPredict->meanTime = stepPredict->meanTime * 0.5f + tmpTime * 0.5f;
|
||||
}else{
|
||||
stepPredict->meanTime = tmpTime;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
stepPredict->fnum = 0;
|
||||
stepPredict->fsum = 0;
|
||||
|
||||
stepPredict->lastTime = timestamp;
|
||||
stepPredict->deltaStep = 0;
|
||||
|
||||
}
|
||||
|
||||
int predictStep(StepPredict *stepPredict, double timestamp, unsigned long steps_last, float gnssVel)
|
||||
{
|
||||
int step = 0;
|
||||
float mean_time = 400;
|
||||
double dis = 0;
|
||||
|
||||
if (stepPredict->meanTime > 0)
|
||||
mean_time = stepPredict->meanTime;
|
||||
|
||||
if ((steps_last > 0) && (stepPredict->fnum > 0) && (timestamp - stepPredict->lastTime > mean_time * 3) && (stepPredict->lastTime > 0))
|
||||
{
|
||||
stepPredict->fsum = stepPredict->fsum / stepPredict->fnum;
|
||||
dis = sqrt(pow(GpsPos[HIST_GPS_NUM - 1][0]- GpsPos[HIST_GPS_NUM - 2][0], 2) + pow(GpsPos[HIST_GPS_NUM - 1][1] - GpsPos[HIST_GPS_NUM - 2][1], 2)
|
||||
+ pow(GpsPos[HIST_GPS_NUM - 1][2] - GpsPos[HIST_GPS_NUM - 2][2], 2));
|
||||
|
||||
if (stepPredict->fsum != 0.0 && (gnssVel > 1.0 || (dis > 1.0 && dis < 3.0)))
|
||||
{
|
||||
step = (int)((timestamp - stepPredict->lastTime) / mean_time);
|
||||
stepPredict->deltaStep += step;
|
||||
}
|
||||
|
||||
stepPredict->lastTime = timestamp;
|
||||
stepPredict->fnum = 0;
|
||||
stepPredict->fsum = 0;
|
||||
}
|
||||
|
||||
return step;
|
||||
}
|
||||
|
||||
/**----------------------------------------------------------------------
|
||||
* Function : stateRecognition
|
||||
* Description : 根据加速度及判断当前状态是否平稳 1: 不平稳 0: 平稳
|
||||
* Date : 2020/7/22 logzhan
|
||||
*---------------------------------------------------------------------**/
|
||||
void stateRecognition(float *acc, classifer *sys) {
|
||||
int i,j,k;
|
||||
float accNorm = 0;
|
||||
float sum = 0, max_min = 0, max = -1, min = 999;
|
||||
float mean = 0, std = 0;
|
||||
|
||||
// 先计算当前加速度的模
|
||||
accNorm = (float)sqrt(vivopow(acc[0], 2) + vivopow(acc[1], 2) + vivopow(acc[2], 2));
|
||||
|
||||
// 缓存更新
|
||||
for (i = 0; i < PATTERN_RECOGNITION_LEN - 1; i++) {
|
||||
sys->accBuffer[i] = sys->accBuffer[i + 1];
|
||||
}
|
||||
sys->accBuffer[PATTERN_RECOGNITION_LEN - 1] = accNorm;
|
||||
|
||||
// 计算最大最小值和sum
|
||||
for (j = 0; j < PATTERN_RECOGNITION_LEN - 1; j++) {
|
||||
sum += sys->accBuffer[j];
|
||||
if (sys->accBuffer[j] > max) {
|
||||
max = sys->accBuffer[j];
|
||||
}
|
||||
if (sys->accBuffer[j] < min) {
|
||||
min = sys->accBuffer[j];
|
||||
}
|
||||
}
|
||||
|
||||
// 计算均值
|
||||
mean = sum / PATTERN_RECOGNITION_LEN;
|
||||
// 计算方差
|
||||
for (k = 0; k < PATTERN_RECOGNITION_LEN - 1; k++) {
|
||||
std += (sys->accBuffer[k] - mean) * (sys->accBuffer[k] - mean) / PATTERN_RECOGNITION_LEN;
|
||||
}
|
||||
std = (float)sqrt(std);
|
||||
|
||||
// 计算最大最小差值
|
||||
max_min = max - min;
|
||||
|
||||
if (std > 5 && max_min > 25) {
|
||||
sys->type = 1;
|
||||
}
|
||||
else {
|
||||
sys->type = 0;
|
||||
}
|
||||
}
|
||||
|
||||
/**----------------------------------------------------------------------
|
||||
* Function : calPredAngle
|
||||
* Description : 根据pdr->cal_direction与gps_yaw角度综合确定角度angle
|
||||
* Date : 2020/7/22 logzhan
|
||||
*---------------------------------------------------------------------**/
|
||||
double calPredAngle(PDR *g_pdr, classifer *sys, double gpsYaw, double trackYaw, imu *ss_data) {
|
||||
int attitude_flag = PDR_FALSE;
|
||||
g_detector = pdr_getDetector();
|
||||
|
||||
/*姿态角YAW辅助计算start*/
|
||||
if (DETECTOR_TYPE_HANDHELD == g_detector->type)
|
||||
{
|
||||
b_timestamp = ss_data->gyr.time;
|
||||
if (!(angle_mean[0] == 0 && angle_mean[1] == 0 && angle_mean[2] == 0))
|
||||
{
|
||||
if (b_last_time > 0 && b_timestamp - b_last_time >= 5000 && gpsYaw != 0.0 && gpsYaw != -1 && trackYaw != -1)
|
||||
{
|
||||
/* 将姿态yaw角调整到0到2*PI */
|
||||
|
||||
/*左臂包，头朝上，屏朝外 or 右臂包，头朝上，屏朝内*/
|
||||
if (ss_data->acc.s[1] > 9 && (fabs(angle_mean[2] + 6.15 - gpsYaw) < PI / 18 && fabs(angle_mean[2] + 6.15 - trackYaw) < PI / 18))
|
||||
{
|
||||
FUR_duration = -1;
|
||||
if (BUL_duration < 0)
|
||||
{
|
||||
BUL_duration = ss_data->gyr.time;
|
||||
}
|
||||
if (BUL_duration >= 0 && (b_timestamp - BUL_duration) >= 1500)
|
||||
{
|
||||
attitude_yaw = angle_mean[2] + 6.15 + PI;
|
||||
attitude_flag = PDR_TRUE;
|
||||
hold_type = BACKWARD_UP_LEFT;
|
||||
}
|
||||
}
|
||||
|
||||
/*右臂包，头朝上，屏朝外 or 左臂包，头朝上，屏朝内*/
|
||||
if (ss_data->acc.s[1] > 9 && (fabs(angle_mean[2] + 7.10 - gpsYaw) < PI / 18 && fabs(angle_mean[2] + 7.10 - trackYaw) < PI / 18))
|
||||
{
|
||||
BUL_duration = -1;
|
||||
if (FUR_duration < 0)
|
||||
{
|
||||
FUR_duration = ss_data->gyr.time;
|
||||
}
|
||||
if (FUR_duration >= 0 && (b_timestamp - FUR_duration) >= 1500)
|
||||
{
|
||||
attitude_yaw = angle_mean[2] + 7.10;
|
||||
attitude_flag = PDR_TRUE;
|
||||
hold_type = FORWARD_UP_RIGHT;
|
||||
}
|
||||
}
|
||||
}
|
||||
if (-1 == b_last_time)
|
||||
{
|
||||
b_last_time = b_timestamp;
|
||||
}
|
||||
}
|
||||
}
|
||||
else{
|
||||
b_last_time = -1;
|
||||
FUR_duration = -1;
|
||||
BUL_duration = -1;
|
||||
}
|
||||
/*姿态角YAW辅助计算end*/
|
||||
|
||||
double angle = -1;
|
||||
double ahrsYaw = atan2(g_pdr->cal_direction[1], g_pdr->cal_direction[0]);
|
||||
|
||||
//printf("PCA Yaw = %f\n", ahrsYaw * (180 / 3.1415926));
|
||||
|
||||
if (ahrsYaw < 0) {
|
||||
ahrsYaw += TWO_PI;
|
||||
}
|
||||
|
||||
// 评估PCA计算方向角的准确性
|
||||
if ((g_pdr->bias_accuracy > 0 && g_pdr->bias_accuracy < 0.9) && (g_pdr->pca_accuracy > 0 && g_pdr->pca_accuracy < 0.25)) {
|
||||
angle = ahrsYaw;
|
||||
//printf("PCA准确性满足条件, angle = ahrsYaw %f\n", ahrsYaw * (180 / 3.1415926));
|
||||
}
|
||||
if (gpsYaw != 0.0 && gpsYaw != -1 ) //&& scene_type
|
||||
{
|
||||
// Status -- Explosion
|
||||
if (fabs(ss_data->acc.s[0]) > 70 || fabs(ss_data->acc.s[1]) > 70 || fabs(ss_data->acc.s[2]) > 70 || fabs(ss_data->gyr.s[0]) > 30 || fabs(ss_data->gyr.s[1]) > 30 || fabs(ss_data->gyr.s[2]) > 30) {
|
||||
angle = gpsYaw;
|
||||
//printf("Explosion, angle = gpsYaw = %f\n", gps_yaw * (180 / 3.1415926));
|
||||
}
|
||||
// Status -- Running
|
||||
|
||||
if (g_pdr->motionFreq > 2.7 || (g_pdr->motionType != 3 && g_pdr->motionType != 2) || (sys->type == 1)) { //&& g_pdr->motion_type != 2
|
||||
angle = gpsYaw;
|
||||
//printf("Running, angle = gpsYaw = %f\n", gps_yaw * (180 / 3.1415926));
|
||||
}
|
||||
}
|
||||
|
||||
if (trackYaw != -1 && ((g_pdr->bias_accuracy < 0 || g_pdr->bias_accuracy > 0.05) || (g_pdr->pca_accuracy < 0 || g_pdr->pca_accuracy > 0.05))) {
|
||||
angle = trackYaw;//gps_yaw: obtained from the datasheet; G_yaw : calculated from the route
|
||||
if (attitude_flag == PDR_TRUE)
|
||||
{
|
||||
angle = attitude_yaw;
|
||||
}
|
||||
}
|
||||
|
||||
if (angle < 0)
|
||||
{
|
||||
if (g_pdr->bias_accuracy > 0 && g_pdr->pca_accuracy > 0 && (g_pdr->bias_accuracy < 1.5 && g_pdr->pca_accuracy < 0.5))//g_pdr->bias_accuracy < 6
|
||||
{
|
||||
angle = ahrsYaw;
|
||||
}
|
||||
else if (attitude_flag == PDR_TRUE)
|
||||
{
|
||||
angle = attitude_yaw;
|
||||
}
|
||||
else if (gpsYaw != 0.0 && gpsYaw != -1)
|
||||
{
|
||||
angle = gpsYaw;
|
||||
}
|
||||
else //if (lastYaw > 0)
|
||||
{
|
||||
angle = -1; //lastYaw;
|
||||
}
|
||||
}
|
||||
|
||||
return angle;
|
||||
}
|
||||
|
||||
|
||||
|
||||
double calGnssTrackHeading(double gpsPos[][3], gnss pgnss)
|
||||
{
|
||||
int i;
|
||||
double lla[3] = { 0.0 };
|
||||
double ned1[3] = { 0.0 };
|
||||
double temp[2][HIST_GPS_NUM] = { { 0.0 } };
|
||||
double angle[HIST_GPS_NUM - 1] = { 0.0 };
|
||||
|
||||
double ned2[3] = { 0.0 };
|
||||
double maxn = 0;
|
||||
double maxe = 0;
|
||||
double minn = 0;
|
||||
double mine = 0;
|
||||
double meanx = 0;
|
||||
double meany = 0;
|
||||
|
||||
float error = 0;
|
||||
float sst = 0;
|
||||
float r2 = 0;
|
||||
|
||||
double range = 0;
|
||||
double yaw = 0;
|
||||
double tempYaw = 0;
|
||||
double diffAngle = 0;
|
||||
|
||||
double para[3] = { 0 };
|
||||
int flg = 0;
|
||||
|
||||
lla[0] = pgnss.lat;
|
||||
lla[1] = pgnss.lon;
|
||||
|
||||
for (i = 0; i < HIST_GPS_NUM - 1; i++)
|
||||
{
|
||||
gpsPos[i][0] = gpsPos[i + 1][0];
|
||||
gpsPos[i][1] = gpsPos[i + 1][1];
|
||||
gpsPos[i][2] = gpsPos[i + 1][2];
|
||||
}
|
||||
|
||||
if ((lla[0] > 0) && (lla[1] > 0))
|
||||
{
|
||||
//经纬度WGS84转地心地固坐标系ECEF
|
||||
WGS842ECEF(lla, gpsPos[HIST_GPS_NUM - 1]);
|
||||
}
|
||||
else
|
||||
{
|
||||
gpsPos[HIST_GPS_NUM - 1][0] = 0;
|
||||
gpsPos[HIST_GPS_NUM - 1][1] = 0;
|
||||
gpsPos[HIST_GPS_NUM - 1][2] = 0;
|
||||
return -1;
|
||||
}
|
||||
if (pgnss.error > 40)
|
||||
{
|
||||
return -1;
|
||||
}
|
||||
|
||||
for (i = 0; i < HIST_GPS_NUM - 1; i++)
|
||||
{
|
||||
if ((fabs(gpsPos[i][0]) < 1e-6) && (fabs(gpsPos[i][1]) < 1e-6) && (fabs(gpsPos[i][2]) < 1e-6))
|
||||
{
|
||||
return -1;
|
||||
}
|
||||
}
|
||||
//地心地固坐标系ECEF转东北天坐标系NED
|
||||
ECEF2Ned(gpsPos[HIST_GPS_NUM - 1], lla, ned1);
|
||||
|
||||
for (i = HIST_GPS_NUM - 2; i >= 0; i--) {
|
||||
|
||||
ECEF2Ned(gpsPos[i], lla, ned2);
|
||||
|
||||
temp[0][i] = ned1[0] - ned2[0];
|
||||
temp[1][i] = ned1[1] - ned2[1];
|
||||
|
||||
meanx += temp[0][i] / HIST_GPS_NUM;
|
||||
meany += temp[1][i] / HIST_GPS_NUM;
|
||||
|
||||
if (maxn > temp[0][i])
|
||||
{
|
||||
maxn = temp[0][i];
|
||||
}
|
||||
if (minn < temp[0][i])
|
||||
{
|
||||
minn = temp[0][i];
|
||||
}
|
||||
if (maxe > temp[1][i])
|
||||
{
|
||||
maxe = temp[1][i];
|
||||
}
|
||||
if (mine < temp[1][i])
|
||||
{
|
||||
mine = temp[1][i];
|
||||
}
|
||||
|
||||
angle[i] = atan2(temp[1][i], temp[0][i]);
|
||||
if (angle[i] < 0)
|
||||
angle[i] += TWO_PI;
|
||||
}
|
||||
flg = leastDistanceLinearFit(temp[0], temp[1], HIST_GPS_NUM, para);
|
||||
if (flg < 0)
|
||||
return -1;
|
||||
|
||||
for (i = HIST_GPS_NUM - 1; i >= 0; i--) {
|
||||
error += (float)(vivopow(para[0] * temp[0][i] + para[1] * temp[1][i] + para[2], 2) / (vivopow(para[0], 2) + vivopow(para[1], 2)));
|
||||
sst += (float)(vivopow(meanx - temp[0][i], 2) + vivopow(meany - temp[1][i], 2));
|
||||
}
|
||||
|
||||
if (sst > 0)
|
||||
{
|
||||
r2 = 1 - error / sst;
|
||||
}
|
||||
range = sqrt(vivopow(maxn - minn, 2) + vivopow(maxe - mine, 2));
|
||||
//printf("%f,%f,%f\n", r2, error, range);
|
||||
if (r2 < 0.95 || range < 1.5 || error > 3.0)
|
||||
return -1;
|
||||
|
||||
yaw = atan(-para[0] / para[1]);
|
||||
modAngle(&yaw, 0, TWO_PI);
|
||||
|
||||
tempYaw = meanAngle(angle, HIST_GPS_NUM - 1);
|
||||
modAngle(&tempYaw, 0, TWO_PI);
|
||||
|
||||
diffAngle = tempYaw - yaw;
|
||||
modAngle(&diffAngle, -PI, PI);
|
||||
|
||||
if (fabs(diffAngle) > PI / 2)
|
||||
yaw += PI;
|
||||
modAngle(&yaw, 0, TWO_PI);
|
||||
|
||||
return yaw;
|
||||
}
|
||||
|
||||
/**----------------------------------------------------------------------
|
||||
* Function : pdr_insPredict
|
||||
* Description : PDR惯导位置更新,
|
||||
* Date : 2020/07/08 logzhan
|
||||
* 2021/02/06 logzhan : 感觉计步器输出不够平均，有时候
|
||||
* 会造成PDR前后位置不够均匀
|
||||
*---------------------------------------------------------------------**/
|
||||
void pdr_insPredict(PDR *g_pdr, StepPredict *stepPredict, imu *ss_data,
|
||||
gnss *pgnss, KfPara *kf) {
|
||||
int step;
|
||||
long deltaStep;
|
||||
double stepLength = 0.7;
|
||||
|
||||
stepPredict->fnum++;
|
||||
stepPredict->fsum += g_pdr->motionFreq;
|
||||
|
||||
ulong steps = g_pdr->steps;
|
||||
ulong lastSteps = g_pdr->lastSteps;
|
||||
|
||||
if (steps - lastSteps > 0 && g_pdr->motionFreq != 0.0) {
|
||||
deltaStep = steps - lastSteps - stepPredict->deltaStep;
|
||||
|
||||
// 应该是防止计步器异常情况，一次计步特别多
|
||||
if (deltaStep > 5)deltaStep = 5;
|
||||
|
||||
if (stepPredict->meanTime > 0)
|
||||
{
|
||||
step = (int)((ss_data->gyr.time - stepPredict->lastTime) / stepPredict->meanTime + 0.5);
|
||||
|
||||
if (step > deltaStep && step <= 3)deltaStep = step;
|
||||
}
|
||||
if (lastSteps == 0)deltaStep = 0;
|
||||
|
||||
if (g_pdr->heading > 0) {
|
||||
pdr_ekfStatePredict(kf, stepLength, g_pdr, deltaStep);
|
||||
g_pdr->fusionPdrFlg = PDR_TRUE;
|
||||
}
|
||||
calStepLastTime(stepPredict, ss_data->gyr.time, lastSteps, steps);
|
||||
}else{
|
||||
// 有时候，计步器可能考虑到稳定性暂时不更新步数，因此需要利用GNSS速度预测步数，达到
|
||||
// 惯导更新位置的目的
|
||||
int preStep = predictStep(stepPredict, ss_data->gyr.time, lastSteps, pgnss->vel);
|
||||
if (g_pdr->heading > 0){
|
||||
pdr_ekfStatePredict(kf, stepLength, g_pdr, preStep);
|
||||
if(preStep > 0)g_pdr->fusionPdrFlg = PDR_TRUE;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/**----------------------------------------------------------------------
|
||||
* Function : resetOutputResult
|
||||
* Description : 1、初始化解算初始位置、卡尔曼滤波初始参数
|
||||
* 2、GPS值赋值给result
|
||||
* Date : 2020/07/08 logzhan
|
||||
*---------------------------------------------------------------------**/
|
||||
int resetOutputResult(gnss *pgnss, PDR* g_pdr, KfPara *kf, lct_fs *result) {
|
||||
|
||||
double stepLength = 0.7;
|
||||
double ned[3] = { 0 };
|
||||
|
||||
g_pdr->pllaInit[0] = pgnss->lat;
|
||||
g_pdr->pllaInit[1] = pgnss->lon;
|
||||
g_pdr->pllaInit[2] = 0;
|
||||
wgs842Ned(g_pdr->pllaInit, g_pdr->pllaInit, ned);
|
||||
|
||||
g_pdr->x0 = ned[0];
|
||||
g_pdr->y0 = ned[1];
|
||||
|
||||
kf->p_xk[0] = ned[0];
|
||||
kf->p_xk[1] = ned[1];
|
||||
kf->p_xk[2] = stepLength;
|
||||
// 重置输出GPS时，GPS的航向角不一定准
|
||||
kf->p_xk[3] = pgnss->yaw*RADIAN_PER_DEG;
|
||||
|
||||
for (uchar i = 0; i < N; i++) {
|
||||
kf->xk[i] = kf->p_xk[i];
|
||||
}
|
||||
|
||||
for (int i = 0; i < N; i++) {
|
||||
for (int l = 0; l < N; l++) {
|
||||
kf->q[i][l] = 0.0;
|
||||
kf->r[i][l] = 0.0;
|
||||
}
|
||||
}
|
||||
//printf("重置PDR滤波器，输出GPS位置\n");
|
||||
// 输出GPS位置结果
|
||||
pdr_outputGpsPos(pgnss, result);
|
||||
g_pdr->noGpsCount = 0;
|
||||
return PDR_TRUE;
|
||||
}
|
||||
|
||||
int pdr_initProc(gnss *pgnss, KfPara *kf, PDR* g_pdr,lct_fs *result)
|
||||
{
|
||||
double stepLen = 0.7; // 初始运动步长为0.7m
|
||||
if (pgnss->satellites_num > 4 && pgnss->HDOP < 2.0 && pgnss->vel >= 1.5 && pgnss->yaw != -1) {
|
||||
// plla 类似位置坐标的原点(单位是经纬度)
|
||||
|
||||
g_pdr->pllaInit[0] = pgnss->lat;
|
||||
g_pdr->pllaInit[1] = pgnss->lon;
|
||||
g_pdr->pllaInit[2] = 0;
|
||||
|
||||
// plla 转 ned
|
||||
double ned[3] = { 0 };
|
||||
wgs842Ned(g_pdr->pllaInit, g_pdr->pllaInit, ned);
|
||||
|
||||
// 在NED坐标系的计算, 单位是米
|
||||
g_pdr->x0 = ned[0];
|
||||
g_pdr->y0 = ned[1];
|
||||
|
||||
// 初始化卡尔曼滤波器的观测量
|
||||
kf->p_xk[0] = ned[0]; // 状态量1: 北向x
|
||||
kf->p_xk[1] = ned[1]; // 状态量2：东向y
|
||||
kf->p_xk[2] = stepLen; // 状态量3：步长
|
||||
kf->p_xk[3] = pgnss->yaw * RADIAN_PER_DEG; // 状态量4：航向角
|
||||
|
||||
for (uchar i = 0; i < N; i++) {
|
||||
kf->xk[i] = kf->p_xk[i];
|
||||
}
|
||||
|
||||
kf->initHeading = kf->p_xk[3];
|
||||
for (int i = 0; i < N; i++) {
|
||||
for (int l = 0; l < N; l++) {
|
||||
kf->q[i][l] = 0.0;
|
||||
kf->r[i][l] = 0.0;
|
||||
}
|
||||
}
|
||||
pdr_outputGpsPos(pgnss, result);
|
||||
g_pdr->sysStatus = IS_NORMAL;
|
||||
|
||||
return PDR_TRUE;
|
||||
}
|
||||
|
||||
if (pgnss->satellites_num > 4 && pgnss->HDOP < 2.0 && pgnss->vel > 0 && pgnss->yaw != -1) {
|
||||
pdr_outputGpsPos(pgnss, result);
|
||||
return PDR_TRUE;
|
||||
}
|
||||
return PDR_FALSE;
|
||||
}
|
||||
|
||||
/**----------------------------------------------------------------------
|
||||
* Function : calEkfQRMatrix
|
||||
* Description : 根据GPS信号特征调整卡尔曼滤波噪声矩阵，q矩阵是过程噪声矩阵，
|
||||
* 需要跟惯导预测位置精度相关。r矩阵为GNSS观测噪声，跟GPS输出的
|
||||
* 信息精度有关。
|
||||
* Date : 2020/07/09 logzhan
|
||||
*---------------------------------------------------------------------**/
|
||||
void calEkfQRMatrix(lct_nmea *nmea, PDR *g_pdr, classifer *sys,
|
||||
KfPara *kf) {
|
||||
|
||||
if (g_pdr->motionType != PDR_MOTION_TYPE_HANDHELD &&
|
||||
g_pdr->motionType != PDR_MOTION_TYPE_STATIC) {
|
||||
kf->q[0][0] = 25; // PDR预测位置噪声
|
||||
kf->q[1][1] = 25; // PDR预测位置噪声
|
||||
kf->q[2][2] = 1; // PDR步长噪声
|
||||
kf->q[3][3] = 100; // PDR方向噪声
|
||||
|
||||
kf->r[0][0] = 50; // GPS观测位置噪声
|
||||
kf->r[1][1] = 50; // GPS观测位置噪声
|
||||
kf->r[2][2] = 0.1; // GPS步长噪声
|
||||
kf->r[3][3] = 50; // GPS方向噪声
|
||||
return;
|
||||
}
|
||||
|
||||
kf->q[0][0] = 0.1; // PDR预测位置噪声
|
||||
kf->q[1][1] = 0.1; // PDR预测位置噪声
|
||||
kf->q[2][2] = 1; // PDR步长噪声
|
||||
kf->q[3][3] = 1; // PDR方向噪声
|
||||
|
||||
kf->r[0][0] = 100; // GPS预测位置噪声
|
||||
kf->r[1][1] = 100; // GPS预测位置噪声
|
||||
kf->r[2][2] = 0.1; // GPS步长噪声
|
||||
kf->r[3][3] = 1000; // GPS方向噪声
|
||||
|
||||
if (nmea->accuracy.err > 0 &&
|
||||
nmea->accuracy.err < 3.80 &&
|
||||
nmea->gga.hdop <= 0.4)
|
||||
{
|
||||
kf->r[0][0] = 50; // GPS观测位置噪声
|
||||
kf->r[1][1] = 50; // GPS观测位置噪声
|
||||
kf->r[2][2] = 0.1; // GPS步长噪声
|
||||
kf->r[3][3] = 50; // GPS方向噪声
|
||||
}
|
||||
|
||||
if (fabs(R2D((g_pdr->gpsHeading - g_pdr->lastGpsHeading))) > 50 ||
|
||||
g_pdr->gpsSpeed < 0.7 || g_pdr->gpsHeading == -1) {
|
||||
kf->r[3][3] = 10000;
|
||||
}
|
||||
}
|
||||
/**----------------------------------------------------------------------
|
||||
* Function : pdr_gnssInsLocFusion
|
||||
* Description : PDR的GNSS和INS融合定位
|
||||
* Date : 2020/07/09 logzhan
|
||||
*---------------------------------------------------------------------**/
|
||||
void pdr_gnssInsLocFusion(gnss *pgnss, PDR *g_pdr, classifer *sys, double yaw_bias,
|
||||
KfPara *kf, lct_fs *res) {
|
||||
double plla[3] = { 0 };
|
||||
double ned[3] = { 0 };
|
||||
double yaw_thr = 6;
|
||||
|
||||
int scene_type = g_pdr->sceneType;
|
||||
|
||||
plla[0] = pgnss->lat;
|
||||
plla[1] = pgnss->lon;
|
||||
plla[2] = 0;
|
||||
|
||||
wgs842Ned(plla, g_pdr->pllaInit, ned);
|
||||
|
||||
pgnss->xNed = ned[0];
|
||||
pgnss->yNed = ned[1];
|
||||
|
||||
//调整融合策略
|
||||
pdr_ekfStateUpdate(kf, pgnss, sys, g_pdr, scene_type);
|
||||
|
||||
for (uchar i = 0; i < N; i++) {
|
||||
for (uchar l = 0; l < N; l++) {
|
||||
kf->p_pk[i][l] = kf->pk[i][l];
|
||||
}
|
||||
kf->p_xk[i] = kf->xk[i];
|
||||
}
|
||||
|
||||
ned[0] = kf -> p_xk[0] - g_pdr->x0;
|
||||
ned[1] = kf -> p_xk[1] - g_pdr->y0;
|
||||
ned[2] = 0;
|
||||
|
||||
ned2Wgs84(g_pdr->pllaInit, ned, plla);
|
||||
|
||||
res->latitude = plla[0];
|
||||
res->latitude_ns = pgnss->lat_ns;
|
||||
res->longitudinal = plla[1];
|
||||
res->longitudinal_ew = pgnss->lon_ew;
|
||||
res->time = pgnss->timestamps;
|
||||
|
||||
g_pdr->noGpsCount = 0;
|
||||
}
|
||||
|
|
@ -1,149 +0,0 @@
|
|||
/******************** (C) COPYRIGHT 2020 Geek************************************
|
||||
* File Name : pdr_linearFit.c
|
||||
* Department : Sensor Algorithm Team
|
||||
* Current Version : V2.0(compare QCOM SAP 5.0)
|
||||
* Author : logzhan
|
||||
* Date of Issued : 2020.7.4
|
||||
* Comments : PDR 线性拟合相关函数支持, 主要是用于轨迹平滑
|
||||
********************************************************************************/
|
||||
/* Header File Including -----------------------------------------------------*/
|
||||
#include <math.h>
|
||||
#include <stdlib.h>
|
||||
#include "pdr_linearFit.h"
|
||||
#include "pdr_util.h"
|
||||
|
||||
/**----------------------------------------------------------------------
|
||||
* Function : pdr_linearLeastSquaresFitting
|
||||
* Description : 线性最小二乘拟合 Y = c0 + c1 x
|
||||
目标函数为sum| nDataY - (a*nDataX+b)|^2最小
|
||||
* Date : 2020/7/4 logzhan
|
||||
*---------------------------------------------------------------------**/
|
||||
int linearLeastSquaresFit(const double *nDataX, const double *nDataY, const int nLength, double *a, double *b)
|
||||
{
|
||||
int fittingRes = 0;
|
||||
if (nDataX == NULL || nDataY == NULL || a == NULL || b == NULL){
|
||||
fittingRes = -1;
|
||||
} else if (nLength <= 0){
|
||||
fittingRes = 1;
|
||||
} else {
|
||||
fittingRes = gslFitLinear(nDataX, 1, nDataY, 1, nLength, b, a, NULL, NULL, NULL, NULL);
|
||||
}
|
||||
return fittingRes;
|
||||
}
|
||||
|
||||
/**----------------------------------------------------------------------
|
||||
* Function : gsl_fit_linear
|
||||
* Description : Fit the data (x_i, y_i) to the linear relationship
|
||||
Y = c0 + c1 x
|
||||
returning,
|
||||
c0, c1 -- coefficients
|
||||
cov00, cov01, cov11 -- variance-covariance matrix of
|
||||
c0 and c1,
|
||||
sumsq -- sum of squares of residuals
|
||||
This fit can be used in the case where the errors for
|
||||
the data are uknown, but assumed equal for all points.
|
||||
The resulting variance-covariance matrix estimates the
|
||||
error in the coefficientsfrom the observed variance of
|
||||
the points around the best fit line.
|
||||
* Date :
|
||||
*---------------------------------------------------------------------**/
|
||||
static int gslFitLinear(const double *x, const size_t xstride,
|
||||
const double *y, const size_t ystride,
|
||||
const size_t n,
|
||||
double *c0, double *c1,
|
||||
double *cov_00, double *cov_01, double *cov_11, double *sumsq)
|
||||
{
|
||||
double m_x = 0, m_y = 0, m_dx2 = 0, m_dxdy = 0;
|
||||
double dx;
|
||||
double dy;
|
||||
size_t i;
|
||||
double s2 = 0, d2 = 0;
|
||||
double b = 0;
|
||||
double a = 0;
|
||||
double d = 0;
|
||||
for (i = 0; i < n; i++)
|
||||
{
|
||||
m_x += (x[i * xstride] - m_x) / (i + 1.0);
|
||||
m_y += (y[i * ystride] - m_y) / (i + 1.0);
|
||||
}
|
||||
|
||||
for (i = 0; i < n; i++)
|
||||
{
|
||||
dx = x[i * xstride] - m_x;
|
||||
dy = y[i * ystride] - m_y;
|
||||
|
||||
m_dx2 += (dx * dx - m_dx2) / (i + 1.0);
|
||||
m_dxdy += (dx * dy - m_dxdy) / (i + 1.0);
|
||||
}
|
||||
|
||||
/* In terms of y = a + b x */
|
||||
if (fabs(m_dx2) < 1e-20)return -1;
|
||||
|
||||
b = m_dxdy / m_dx2;
|
||||
a = m_y - m_x * b;
|
||||
|
||||
*c0 = a;
|
||||
*c1 = b;
|
||||
|
||||
/* Compute chi^2 = \sum (y_i - (a + b * x_i))^2 */
|
||||
for (i = 0; i < n; i++)
|
||||
{
|
||||
dx = x[i * xstride] - m_x;
|
||||
dy = y[i * ystride] - m_y;
|
||||
d = dy - b * dx;
|
||||
d2 += d * d;
|
||||
}
|
||||
|
||||
s2 = d2 / (n - 2.0); /* chisq per degree of freedom */
|
||||
|
||||
/* 为函数增加空指针检查以使函数可以将一下参数设置为空指针 */
|
||||
if (cov_00 != NULL)
|
||||
*cov_00 = s2 * (1.0 / n) * (1 + m_x * m_x / m_dx2);
|
||||
if (cov_11 != NULL)
|
||||
*cov_11 = s2 * 1.0 / (n * m_dx2);
|
||||
if (cov_01 != NULL)
|
||||
*cov_01 = s2 * (-m_x) / (n * m_dx2);
|
||||
|
||||
if (sumsq != NULL)
|
||||
*sumsq = d2;
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
|
||||
/**----------------------------------------------------------------------
|
||||
* Function : leastDistanceLinearFit
|
||||
* Description : 二维坐标系点的线性拟合，ax+by+c=0, 目标函数为点到拟合直线的距离
|
||||
平方和最小， 这个函数的计算量还是相对较大的，主要注意这个函数不
|
||||
适合频繁调用
|
||||
* Input : x， y，二维坐标X和Y
|
||||
num, 二维点个数
|
||||
* Output : para， 拟合直线参数, para[0]:a, para[0]:b, para[0]:c
|
||||
*---------------------------------------------------------------------**/
|
||||
int leastDistanceLinearFit(double x[], double y[], int num, double para[])
|
||||
{
|
||||
int i;
|
||||
double covMatrix[2][2] = { { 0.0 } };
|
||||
double p[2][2] = { { 0.0 } };
|
||||
|
||||
if (num < 2)
|
||||
return -1;
|
||||
|
||||
getCovMatrix(x, y, covMatrix, num);
|
||||
Jacobi(covMatrix, p, 2, 1e-10, 1000);
|
||||
|
||||
if (covMatrix[0][0] > covMatrix[1][1]) {
|
||||
para[0] = p[0][1];
|
||||
para[1] = p[1][1];
|
||||
}
|
||||
else {
|
||||
para[0] = p[0][0];
|
||||
para[1] = p[1][0];
|
||||
}
|
||||
|
||||
para[2] = 0;
|
||||
for (i = num - 1; i >= 0; i--) {
|
||||
para[2] -= (para[0] * x[i] + para[1] * y[i]) / num;
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
|
|
@ -1,402 +0,0 @@
|
|||
/******************** (C) COPYRIGHT 2020 Geek************************************
|
||||
* File Name : pdr_location.c
|
||||
* Current Version : V1.2
|
||||
* Author : logzhan
|
||||
* Date of Issued : 2020.7.3
|
||||
* Comments : PDR导航算法主流程
|
||||
********************************************************************************/
|
||||
/* Header File Including -----------------------------------------------------*/
|
||||
#include <stdio.h>
|
||||
#include <math.h>
|
||||
#include <string.h>
|
||||
#include <malloc.h>
|
||||
#include <errno.h>
|
||||
#include <stdlib.h>
|
||||
#include <stdbool.h>
|
||||
#include "pdr_location.h"
|
||||
#include "pdr_kalman.h"
|
||||
#include "steps.h"
|
||||
#include "pdr_ahrs.h"
|
||||
#include "pdr_detector.h"
|
||||
#include "pdr_util.h"
|
||||
#include "scene_recognition.h"
|
||||
|
||||
/* Global Variable Definition ------------------------------------------------*/
|
||||
PDR* pdr; // PDR全局信息
|
||||
StepPredict stepPredict = { 0 };
|
||||
classifer sys;
|
||||
gnss pgnss; // 定位相关信息
|
||||
KfPara g_kfPara;
|
||||
double GpsPos[HIST_GPS_NUM][3] = {{0}};
|
||||
static uint32_t g_status;
|
||||
extern AHRS g_ahrs;
|
||||
|
||||
double refGpsYaw = -1000;
|
||||
double yaw_bias = -1;
|
||||
|
||||
float dir[2] = { 0 };
|
||||
float bias_dir[2] = { 0 };
|
||||
|
||||
/**---------------------------------------------------------------------
|
||||
* Function : pdr_navSysInit
|
||||
* Description : PDR导航系统初始化
|
||||
* Date : 2020/2/1 logzhan
|
||||
*---------------------------------------------------------------------**/
|
||||
void NavSys_Init(void) {
|
||||
|
||||
memset(&g_kfPara, 0, sizeof(g_kfPara));
|
||||
memset(&pgnss, 0, sizeof(pgnss));
|
||||
memset(GpsPos, 0, sizeof(GpsPos));
|
||||
memset(&stepPredict, 0, sizeof(stepPredict));
|
||||
memset(&sys, 0, sizeof(sys));
|
||||
memset(dir, 0, sizeof(dir));
|
||||
memset(bias_dir, 0, sizeof(bias_dir));
|
||||
|
||||
// GNSS定位初始化
|
||||
pdr_initGnssInfo();
|
||||
// 初始化AHRS姿态解算相关
|
||||
pdr_initAhrs();
|
||||
// 检测器初始化
|
||||
pdr_initDetector();
|
||||
// PDR初始化
|
||||
pdr = pdr_initBase();
|
||||
//base文件中姿态角计算初始化
|
||||
pdr_initKalman();
|
||||
// 场景识别模块初始化
|
||||
initSceneRecognition();
|
||||
|
||||
yaw_bias = -1;
|
||||
}
|
||||
|
||||
/**---------------------------------------------------------------------
|
||||
* Function : pdr_initGnssInfo
|
||||
* Description : PDS GNSS定位信息初始化
|
||||
* Date : 2020/02/01 logzhan:
|
||||
* 创建初始版本
|
||||
* 2020/02/08: logzhan 修改initGps为initGnssInfo，更
|
||||
* 加符合函数功能
|
||||
*---------------------------------------------------------------------**/
|
||||
void pdr_initGnssInfo(void) {
|
||||
pgnss.lastError = ACCURACY_ERR_MAX;
|
||||
pgnss.overVelCount = 0;
|
||||
}
|
||||
|
||||
/**---------------------------------------------------------------------
|
||||
* Function : pdr_resetSysStatus
|
||||
* Description : PDR 重设系统状态
|
||||
* Date : 2020/2/1 logzhan
|
||||
*---------------------------------------------------------------------**/
|
||||
void pdr_resetSysStatus(KfPara *kf)
|
||||
{
|
||||
memset(kf, 0, sizeof(KfPara));
|
||||
pdr->fusionPdrFlg = PDR_FALSE;
|
||||
pdr->sysStatus = IS_INITIAL;
|
||||
pgnss.overVelCount = 0;
|
||||
}
|
||||
|
||||
/**---------------------------------------------------------------------
|
||||
* Function : pdr_insLocation
|
||||
* Description : PDR 惯性导航定位
|
||||
* Date : 2020/2/1 logzhan
|
||||
*---------------------------------------------------------------------**/
|
||||
int pdr_insLocation(imu* ss_data, KfPara* kf) {
|
||||
|
||||
// AHRS姿态更新占2%的运行时间
|
||||
if (pdr_imuUpdateAhrs(ss_data)) {
|
||||
// 完成姿态估计后计算PCA， 占26%的运行时间
|
||||
pdr_computePCA(&g_ahrs);
|
||||
}
|
||||
// 更新用户运动类型检测器IMU信息以及用户运动类型检测, 占21%的运行时间
|
||||
updateDetectorImu(ss_data);
|
||||
|
||||
pdr->gyroTime = ss_data->gyr.time;
|
||||
// 为什么手持类型是0.1的运动频率？
|
||||
if (pdr->motionType == DETECTOR_TYPE_HANDHELD)pdr->motionFreq = 0.1f;
|
||||
|
||||
// 计步器输入传感器数据，占10%的运行时间
|
||||
updatePedometer(ss_data, &pdr->steps);
|
||||
|
||||
// ----- Filter Initialization ------- //
|
||||
if (pdr->sysStatus == IS_INITIAL) {
|
||||
pdr->ts = ss_data->gyr.time;
|
||||
pdr->lastSteps = pdr->steps;
|
||||
stepPredict.lastTime = 0;
|
||||
stepPredict.fnum = 0;
|
||||
stepPredict.fsum = 0;
|
||||
return 0;
|
||||
}
|
||||
|
||||
pdr->ts = ss_data->gyr.time;
|
||||
|
||||
if ((0 == ss_data->acc.s[0] && 0 == ss_data->acc.s[1] && 0 == ss_data->acc.s[2]) ||
|
||||
(0 == ss_data->mag.s[0] && 0 == ss_data->mag.s[1] && 0 == ss_data->mag.s[2])) {
|
||||
return 0;
|
||||
}
|
||||
|
||||
// 识别当前状态是否平稳，这个函数计算量到达整个系统10%的计算量, 以后根据情况进行优化
|
||||
// stateRecognition(ss_data->acc.s, &sys);
|
||||
|
||||
pdr->heading = calPredAngle(pdr, &sys, pdr->gpsHeading, pdr->trackHeading, ss_data);
|
||||
if (yaw_bias != -1) {
|
||||
if (pdr->pca_accuracy > 0 && pdr->pca_accuracy < 0.1 && pdr->motionType == 3 &&
|
||||
(pdr->bias_accuracy > 1.5 || pdr->bias_accuracy == -1 || pgnss.error > 30)
|
||||
&& (fabs(yaw_bias - 2 * PI) < YAW_THRES * PI / 180 || fabs(yaw_bias - PI) < YAW_THRES * PI / 180 ||
|
||||
fabs(yaw_bias) < YAW_THRES * PI / 180)) {
|
||||
dir[0] = pdr->pca_direction[0] * bias_dir[0] + pdr->pca_direction[1] * bias_dir[1];
|
||||
dir[1] = pdr->pca_direction[1] * bias_dir[0] - pdr->pca_direction[0] * bias_dir[1];
|
||||
pdr->heading = atan2(dir[1], dir[0]);
|
||||
if (pdr->heading < 0) {
|
||||
pdr->heading += TWO_PI;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
double imuYaw = - g_ahrs.yaw;
|
||||
if (imuYaw < 0) {
|
||||
imuYaw += 360;
|
||||
}
|
||||
|
||||
pdr->heading = imuYaw / (180 / 3.14159265);
|
||||
if (fabs(pdr->insHeadingOffset) > 0.000001) {
|
||||
imuYaw = imuYaw - pdr->insHeadingOffset * (180 / 3.14159265);
|
||||
pdr->heading = imuYaw / (180 / 3.14159265);
|
||||
}
|
||||
|
||||
// PDR 惯性位置预测
|
||||
pdr_insPredict(pdr, &stepPredict, ss_data, &pgnss, kf);
|
||||
|
||||
pdr->lastSteps = pdr->steps;
|
||||
pdr->lastHeading = pdr->heading;
|
||||
return 0;
|
||||
}
|
||||
|
||||
/**----------------------------------------------------------------------
|
||||
* Function : pdr_gnssInsLocation
|
||||
* Description : PDR GPS融合INS惯性定位
|
||||
* Date : 2021/01/29 logzhan
|
||||
*---------------------------------------------------------------------**/
|
||||
int pdr_gnssInsLocation(lct_nmea *nmea_data, KfPara *kf, lct_fs *result)
|
||||
{
|
||||
// GPS数据更新
|
||||
pdr_gnssUpdate(&pgnss, nmea_data);
|
||||
// 检查时间差是否满足条件
|
||||
if (fabs(pgnss.timestamps - pdr->ts) >= 1000)return TYPE_FIX_NONE;
|
||||
// 根据nmea数据识别场景是否是开阔天空场景
|
||||
pdr->sceneType = isOpenArea(nmea_data);
|
||||
// 根据HDOP、速度、卫星数量等判断gps yaw是否可用
|
||||
|
||||
pdr->gpsSpeed = pgnss.vel * GPS_SPEED_UNIT;
|
||||
pdr->lastGpsHeading = pdr->gpsHeading;
|
||||
pdr->gpsHeading = pdr_getGpsHeading(&pgnss);
|
||||
|
||||
// 如果在win32平台下仿真调试,则添加下面代码
|
||||
#ifdef _WIN32
|
||||
result->pdrHeading = kf->p_xk[3];
|
||||
result->gpsHeading = pdr->gpsHeading;
|
||||
result->hdop = pgnss.HDOP;
|
||||
result->accuracy = nmea_data->accuracy.err;
|
||||
result->gpsSpeed = pdr->gpsSpeed;
|
||||
result->motionType = pdr->motionType;
|
||||
// printf("utc time = %f\n", nmea_data->gga.gga_utc);
|
||||
#endif
|
||||
|
||||
calPdrHeadingOffset(nmea_data, pdr);
|
||||
|
||||
// 计算两次GPS时间之间，步数变化量
|
||||
pdr->deltaStepsPerGps = pdr->steps - pdr->lastGpsSteps;
|
||||
pdr->lastGpsSteps = pdr->steps;
|
||||
|
||||
int mode = pdr_detectFixMode(&pgnss, kf, pdr, result);
|
||||
if (mode != TYPE_FIX_PDR)return mode;
|
||||
|
||||
//根据历史GPS拟合输出预估的GPS轨迹航向角
|
||||
pdr->trackHeading = calGnssTrackHeading(GpsPos, pgnss);
|
||||
// 如果系统没有初始化，则先初始化
|
||||
if (pdr->sysStatus == IS_INITIAL) {
|
||||
return pdr_initProc(&pgnss, kf, pdr, result);
|
||||
}
|
||||
// 如果惯导没有达到输出条件，则不预测轨迹
|
||||
if (!pdr->fusionPdrFlg)return resetOutputResult(&pgnss, pdr, kf, result);
|
||||
// 根据GPS信息, 计算kf的过程噪声和观测噪声
|
||||
calEkfQRMatrix(nmea_data, pdr, &sys, kf);
|
||||
|
||||
if (pgnss.satellites_num > 4){
|
||||
// 如果GPS航向角处于无效状态且速度也是无效状态
|
||||
if (pgnss.yaw == INVAILD_GPS_YAW || pgnss.vel <= 0.0) {
|
||||
//if(pdr->heading != INVAILD_GPS_YAW){
|
||||
// pgnss.yaw = (float)(R2D(pdr->heading));
|
||||
//}else if (pdr->trackHeading != INVAILD_GPS_YAW) {
|
||||
// pgnss.yaw = (float)(R2D(pdr->trackHeading));
|
||||
//}
|
||||
pgnss.yaw = (float)kf->p_xk[3];
|
||||
}
|
||||
if (pgnss.yaw != INVAILD_GPS_YAW) {
|
||||
pdr_gnssInsLocFusion(&pgnss, pdr, &sys, yaw_bias, kf, result);
|
||||
return TYPE_FIX_PDR;
|
||||
}
|
||||
}
|
||||
|
||||
// 不满足推算条件，直接输出原始GPS
|
||||
if (pgnss.satellites_num > 4 && nmea_data->accuracy.err > 0 &&
|
||||
nmea_data->accuracy.err < 15) {
|
||||
pdr_outputGpsPos(&pgnss, result);
|
||||
pdr->noGpsCount = 0;
|
||||
return TYPE_FIX_GPS;
|
||||
}
|
||||
|
||||
// 如果没有GPS信号，那么PDR也会接着推算位置, 但是最多推算10个点
|
||||
if (pdr->noGpsCount < MAX_NO_GPS_PREDICT) {
|
||||
// 无GPS状态位置推算
|
||||
pdr_noGpsPredict(kf, result, pdr);
|
||||
return TYPE_FIX_PDR;
|
||||
}
|
||||
pdr->noGpsCount++;
|
||||
return 0;
|
||||
}
|
||||
|
||||
|
||||
/**----------------------------------------------------------------------
|
||||
* Function : pdr_noGpsPredict
|
||||
* Description : 在没有gps信息时预测GPS位置，最多预测10个点
|
||||
* Date : 2020/07/08
|
||||
*
|
||||
*---------------------------------------------------------------------**/
|
||||
void pdr_noGpsPredict(KfPara* kf, lct_fs* result, PDR* p_pdr)
|
||||
{
|
||||
double ned[3] = {0}, lla[3] = {0};
|
||||
ned[0] = kf->p_xk[0] - p_pdr->x0;
|
||||
ned[1] = kf->p_xk[1] - p_pdr->y0;
|
||||
ned[2] = 0;
|
||||
|
||||
ned2Wgs84(p_pdr->pllaInit, ned, lla);
|
||||
|
||||
result->latitude = lla[0];
|
||||
result->latitude_ns = pgnss.lat_ns;
|
||||
result->longitudinal = lla[1];
|
||||
result->longitudinal_ew = pgnss.lon_ew;
|
||||
result->time = pgnss.timestamps;
|
||||
p_pdr->noGpsCount++;
|
||||
}
|
||||
|
||||
/**----------------------------------------------------------------------
|
||||
* Function : calPdrHeadingOffset
|
||||
* Description : 利用GPS信号较好时的偏航角修正磁力计计算方向键的偏移
|
||||
* Date : 2020/07/08 logzhan
|
||||
*---------------------------------------------------------------------**/
|
||||
void calPdrHeadingOffset(lct_nmea* nmea_data, PDR* p_pdr) {
|
||||
//惯导姿态角与GPS行进角度的对准（判断GPS信号是否正常以及惯导是否平稳）
|
||||
if (refGpsYaw != -1000)return;
|
||||
|
||||
if ((pgnss.satellites_num > 4 && pgnss.HDOP < 2.0 && pgnss.yaw != -1 && pgnss.vel >= 1.0)
|
||||
&& (nmea_data->accuracy.err > 0 && nmea_data->accuracy.err < 4)
|
||||
&& (pdr_detStatic(pdr, &pgnss, (p_pdr->steps - p_pdr->lastSteps)) == 0 && pdr->motionType == 2))
|
||||
{
|
||||
/*这里放姿态角与GPS对准part1 start*/
|
||||
double imuHeading = atan2(2 * (g_ahrs.qnb[0] * g_ahrs.qnb[3] + g_ahrs.qnb[1] * g_ahrs.qnb[2]),
|
||||
1 - 2 * (g_ahrs.qnb[2] * g_ahrs.qnb[2] + g_ahrs.qnb[3] * g_ahrs.qnb[3]));
|
||||
if (imuHeading < 0) {
|
||||
imuHeading += TWO_PI;
|
||||
}
|
||||
if (fabs(imuHeading - p_pdr->gpsHeading) < PI / 18)
|
||||
{
|
||||
//printf("offset 标定\n");
|
||||
//p_pdr->insHeadingOffset = imuHeading - p_pdr->gpsHeading;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/**----------------------------------------------------------------------
|
||||
* Function : pdr_detectFixMode
|
||||
* Description : 检测当前PDR处于的模式，如果是车载和静止模式,根据情况选择输出
|
||||
* 原始GPS或者不输出
|
||||
* Date : 2020/07/08 logzhan
|
||||
* 2020/02/08 logzhan : 修改-1为INVAILD_GPS_YAW，提高
|
||||
* 代码的可读性
|
||||
*---------------------------------------------------------------------**/
|
||||
int pdr_detectFixMode(gnss* pgnss, KfPara* kf, PDR* g_pdr, lct_fs* result) {
|
||||
// 检测用户是否处于静止状态
|
||||
if (pdr_detStatic(pdr, pgnss, g_pdr->deltaStepsPerGps)) {
|
||||
if (pgnss->error > 0 && pgnss->error < 15 &&
|
||||
pgnss->lastError >= pgnss->error) {
|
||||
pdr_outputGpsPos(pgnss, result);
|
||||
pgnss->lastError = pgnss->error;
|
||||
return TYPE_FIX_PDR;
|
||||
}
|
||||
// 否则不输出位置
|
||||
return TYPE_FIX_NONE;
|
||||
}
|
||||
// 如果是检测到是开车状态则输出GPS位置
|
||||
if (detIsCarMode(pgnss, pdr, pdr->deltaStepsPerGps, &pgnss->overVelCount)) {
|
||||
// 如果检测到车载模式，则重置PDR状态，需要重新初始化
|
||||
pdr_resetSysStatus(kf);
|
||||
if (pgnss->error >= 0 || (pgnss->satellites_num > 4
|
||||
&& pgnss->HDOP < 2.0)) {
|
||||
pdr_outputGpsPos(pgnss, result);
|
||||
return TYPE_FIX_GPS;
|
||||
}
|
||||
// 精度过差，不输出位置
|
||||
return TYPE_FIX_NONE;
|
||||
}
|
||||
return TYPE_FIX_PDR;
|
||||
}
|
||||
|
||||
|
||||
/**----------------------------------------------------------------------
|
||||
* Function : pdr_getGpsHeading
|
||||
* Description : 获取GPS偏航角(0 - 360°)
|
||||
* Date : 2020/07/08 logzhan
|
||||
* 2020/02/08 logzhan : 修改-1为INVAILD_GPS_YAW，提高
|
||||
* 代码的可读性
|
||||
*---------------------------------------------------------------------**/
|
||||
double pdr_getGpsHeading(gnss* pgnss) {
|
||||
double gpsYaw = INVAILD_GPS_YAW;
|
||||
if (pgnss->satellites_num > 4 && pgnss->HDOP < 2.0 && pgnss->vel >= 1.0
|
||||
&& pgnss->yaw != INVAILD_GPS_YAW && pgnss->error < 50) {
|
||||
gpsYaw = pgnss->yaw * RADIAN_PER_DEG;
|
||||
}
|
||||
return gpsYaw;
|
||||
}
|
||||
|
||||
/**----------------------------------------------------------------------
|
||||
* Function : pdr_nmea2Gnss
|
||||
* Description : nmea数据结构转gnss数据
|
||||
* Date : 2020/07/08 logzhan
|
||||
*---------------------------------------------------------------------**/
|
||||
void pdr_nmea2Gnss(lct_nmea* nmea_data, gnss* pgnss)
|
||||
{
|
||||
lct_nmea_rmc rmc = { 0 };
|
||||
if (nmea_data->rmc[0].time > nmea_data->rmc[1].time) {
|
||||
rmc = nmea_data->rmc[0];
|
||||
}else {
|
||||
rmc = nmea_data->rmc[1];
|
||||
}
|
||||
// 更新上一次的Accuracy Error
|
||||
pgnss->lastError = pgnss->error;
|
||||
pgnss->lat = rmc.latitude * PI / 180;
|
||||
pgnss->lat_ns = rmc.rmc_latitude_ns;
|
||||
pgnss->lon = rmc.longitudinal * PI / 180;
|
||||
pgnss->lon_ew = rmc.longitudinal_ew;
|
||||
//pgnss->vel = rmc.speed * GPS_SPEED_UNIT;
|
||||
pgnss->vel = rmc.speed;
|
||||
pgnss->HDOP = nmea_data->gsa[0].hdop;
|
||||
pgnss->yaw = rmc.cog;
|
||||
pgnss->quality = 0;
|
||||
pgnss->error = nmea_data->accuracy.err;
|
||||
pgnss->timestamps = rmc.time;
|
||||
pgnss->satellites_num = nmea_data->gga.satellites_nb;
|
||||
|
||||
}
|
||||
/**----------------------------------------------------------------------
|
||||
* Function : pdr_gnssUpdate
|
||||
* Description : nmea数据结构转gnss数据
|
||||
* Date : 2020/07/08 logzhan
|
||||
* 2020/02/09 logzhan : 函数对应原pdr_gpsUpdate函数，
|
||||
* 经过引用分析发现每次GPS更新拷贝gnss结构体和nmea结构体其实没
|
||||
* 什么用，还增加运算量
|
||||
*---------------------------------------------------------------------**/
|
||||
void pdr_gnssUpdate(gnss* gps, lct_nmea* nmea) {
|
||||
// nmea数据转gnss数据结构
|
||||
pdr_nmea2Gnss(nmea, &pgnss);
|
||||
// GPS回调更新，暂时不清楚函数功能
|
||||
gpsUpdateCb(gps);
|
||||
}
|
||||
|
|
@ -1,483 +0,0 @@
|
|||
/******************** (C) COPYRIGHT 2020 Geek************************************
|
||||
* File Name : pdr_main.cpp
|
||||
* Department : Sensor Algorithm Team
|
||||
* Current Version : V2.0
|
||||
* Author : logzhan
|
||||
* Date of Issued : 2021.1.28
|
||||
* Comments : PDR PC端仿真主文件
|
||||
* Modify : 2021/02/04 logzhan : 优化kml显示，kml增加航向角图形
|
||||
* 显示，hdop、heading、accuracy信息在Google Map的显示，方便调
|
||||
* 试代码。
|
||||
* 2021/02/18 logzhan : 增加kml的时间显示，能够拖动进
|
||||
* 度条显示轨迹。可以对比查看轨迹效果。
|
||||
*
|
||||
* 一、优化方向和工作内容
|
||||
* 1) 要跟品质那边谈高通效果对比版本SAP5.0
|
||||
* 2）能够兼容老版本，2个月能够更新版本，能够多次迭代测试
|
||||
* 2021/02/09 logzhan ：发现PDR部分bug记录
|
||||
* 1) GPS速度实际需要乘以0.51444的，但是除了滤波器计算步频之外，
|
||||
* 其他地方都是用的实际的GPS速度，包括步数预测
|
||||
* 2) 惯性导航更新部分buffer mean使用的太多了, 计算平均没有优化
|
||||
* 时间分析发现，buffer mean耗费了大量时间
|
||||
* 2021/02/10 logzhan:
|
||||
* 1) 需要跟品质谈高通对比版本，防止最后效果和高通SAP5.0持平后，
|
||||
* 高通出了新版本，品质不认可效果改进。
|
||||
* 2) 目前GPS的速度是没有乘以0.51444的，许多地方计算没有统一，需
|
||||
* 要统一。
|
||||
* 3）惯性导航更新部分buffer mean使用的太多了, 计算平均没有优化
|
||||
* 时间分析发现，buffer mean耗费了大量时间
|
||||
* 4)*目前在不依赖磁力计的版本也能达到比以前有磁力计效果还好。但是
|
||||
* 磁力计依然是一个重要的辅助确定方向的手段。所以利用GPS航向作为
|
||||
* 观测，磁力计启动动态校准，校准完成后才启动辅助磁力计校准方向，
|
||||
* 或者作为一个观测量，用于提升预测方向的准确程度。
|
||||
* 5)*动态偏移对准，后续会开始开发PCA方向估计，那么PCA估计方向肯定
|
||||
* 和真实方向存在一定的偏差，需要动态对准偏移。
|
||||
* 6）目前的计步器，输出不够实时，对于步长的估计不够准确。目前测试
|
||||
* 的情况是，加入动态步长估计后貌似轨迹会偏大，是不是计步偏少？
|
||||
* 7）这个步频计算用到了fft，感觉没有太大作用，可以考虑在新版本中
|
||||
* 去除，然后以后开发时如果需要可以参考原理重新优化。
|
||||
* 8) 赖工提到路网信息，目前比较确定的是能拿到路的方向信息，考虑
|
||||
* 在选定的场景标定一下经纬度和最近的道路方向信息，观察如何提升
|
||||
* 定位精度(方向辅助，减少方向上的漂移)。
|
||||
* 9) PDR 目前一定概率初始对准方向会偏，所以初始对准时，检测是否
|
||||
* 是直线路径，然后考虑用轨迹计算平均方向角，辅助初始化。
|
||||
* 10) 跟张经睿讨论过，场景识别模块以前主要是针对以前的单频手机，
|
||||
* 所以这个模块考虑重构，因为以前单频手机的参数和现在双频手机不同。
|
||||
* 需要重新优化。
|
||||
* 11) 目前PDR算法在观测方向时，还是一定概率会被带偏，所以导致转弯
|
||||
* 转向不够，然而又不够相信GPS方向，考虑通过多次GPS方向角方差不大时
|
||||
* 调整GPS方向角的噪声策略。
|
||||
* 12) kml支持时间信息，可以拖动进度条比较轨迹同时运行的情况 ---- logzhan finish
|
||||
*
|
||||
* 二、调整规划为：
|
||||
第一期：手持稳定优化 + 摆手(平滑模块主要生效) + 多圈优化 => 不会比之前效果差，部分场景提升
|
||||
第二期：手持稳定优化 + 摆手(PCA + 平滑) + 多圈 => 在摆手场景优化，覆盖一大半场景
|
||||
第三期：手持稳定优化 + 摆手 + 任意动作 => 鲁棒性优化，覆盖全部使用场景
|
||||
第四期：手持稳定优化 + 摆手 + 任意动作 + 多圈 + 真值仪 => 保证轨迹优化和方向优化条件下，精度提升，可以方便对外
|
||||
********************************************************************************/
|
||||
#include <stdio.h>
|
||||
#include <string>
|
||||
#include <errno.h>
|
||||
#include <stdlib.h>
|
||||
#include "pdr_api.h"
|
||||
#include "pdr_main.h"
|
||||
#include "time.h"
|
||||
using namespace std;
|
||||
|
||||
/* Global Variable Definition ------------------------------------------------*/
|
||||
ResultTracks resTracks;
|
||||
FILE* fpSimulatorFile = NULL;
|
||||
/* Extern Variable Definition ------------------------------------------------*/
|
||||
extern "C" PDR g_pdr;
|
||||
extern "C" double refGpsYaw;
|
||||
|
||||
/**----------------------------------------------------------------------
|
||||
* Function : main
|
||||
* Description : pdr算法仿真用的main函数
|
||||
* Date : 2021/01/25 logzhan
|
||||
*---------------------------------------------------------------------**/
|
||||
int main(int argc, char* argv[])
|
||||
{
|
||||
string logPath = "E:\\Software Project\\GnssIns-PDR-Private\\1.Software\\Debug\\";
|
||||
string logDate = "shenzhen";
|
||||
string logLabel = "data\\";
|
||||
string fileHead;
|
||||
char line[256] = { 0 };
|
||||
|
||||
// 解析命令行, 用于解析参数
|
||||
for (int i = 0; i < argc; i++) {
|
||||
string s = argv[i];
|
||||
if(i == 1)logPath = s;
|
||||
if(i == 2)logDate = s;
|
||||
}
|
||||
|
||||
clock_t t = clock();
|
||||
// 获取kml输出路径
|
||||
string kmlPath = logPath + logDate + "\\output\\";
|
||||
// 仿真文件夹路径和Catalog.txt路径
|
||||
string filePath = logPath + logDate + "\\" + logLabel + "\\";
|
||||
string catalogPath = filePath + "catalog.txt";
|
||||
|
||||
FILE* catalogFp = fopen(catalogPath.c_str(), "r");
|
||||
if (catalogFp == NULL){
|
||||
printf("open catalog.txt file %s error %s\n", catalogPath.c_str(),
|
||||
strerror(errno));
|
||||
system("pause");
|
||||
return -1;
|
||||
}
|
||||
|
||||
while (!feof(catalogFp))
|
||||
{
|
||||
FILE* fileFp = getSimulateFile(catalogFp, filePath, fileHead);
|
||||
// PDR 仿真流程
|
||||
Algorithm_Init();
|
||||
|
||||
lct_fs lct_fusion;
|
||||
memset(&lct_fusion,0, sizeof(lct_fusion));
|
||||
memset(&resTracks, 0, sizeof(resTracks));
|
||||
|
||||
// 解析文本仿真
|
||||
while (fgets(line, 256, fileFp)){
|
||||
// 解析文本数据仿真
|
||||
if (pdr_locationMainLoopStr(line, &lct_fusion) != TYPE_FIX_NONE){
|
||||
// 更新结果轨迹，用于最后的kml生成
|
||||
updateResTrack(resTracks, lct_fusion);
|
||||
}
|
||||
}
|
||||
// 关闭PDR算法，释放资源
|
||||
pdr_closeAlgorithm();
|
||||
// 写结果kml
|
||||
KmlWrite(kmlPath, fileHead, string("_PDR_ZL_2.0b"));
|
||||
// 关闭文件
|
||||
fclose(fileFp);
|
||||
}
|
||||
// 打印仿真时间信息
|
||||
printf("PDR simulate spends %f s\n", ((double)(clock() - t) / CLOCKS_PER_SEC));
|
||||
system("pause");
|
||||
return 0;
|
||||
}
|
||||
|
||||
/**----------------------------------------------------------------------
|
||||
* Function : getSimulateFile
|
||||
* Description : 给定文件名，以及文件路径，获取仿真文件的文件指针
|
||||
* catalogFp : 目录文件的文件指针
|
||||
* path_file ：仿真文件的文件目录
|
||||
* fileHead : 通过C++引用返回的去除了扩展名的文件名称
|
||||
* Date : 2021/01/25 logzhan
|
||||
*---------------------------------------------------------------------**/
|
||||
FILE* getSimulateFile(FILE* catalogFp, string path_file, string& fileHead) {
|
||||
|
||||
char file_name[256] = { 0 };
|
||||
FILE* fp = NULL;
|
||||
|
||||
if (fscanf(catalogFp, "%s\n", file_name) == -1){
|
||||
printf("get data file name failed\n");
|
||||
system("pause");
|
||||
exit(-1);
|
||||
}
|
||||
string fileNamePath = path_file + string(file_name);
|
||||
fp = fopen(fileNamePath.c_str(), "rb");
|
||||
if (fp == NULL){
|
||||
printf("open file %s failed error %s\n", fileNamePath.c_str(), strerror(errno));
|
||||
system("pause");
|
||||
exit(-1);
|
||||
}
|
||||
// 去除文件扩展名.csv
|
||||
fileHead = file_name;
|
||||
fileHead = fileHead.substr(0, fileHead.length() - 4);
|
||||
|
||||
printf("%s\n", fileNamePath.c_str());
|
||||
return fp;
|
||||
}
|
||||
|
||||
/**----------------------------------------------------------------------
|
||||
* Function : gpsYaw2GoogleYaw
|
||||
* Description : 为了让kml显示的角度方位正常，需要把0-360顺时针旋转的Yaw转换
|
||||
* 为谷歌支持的Yaw规则
|
||||
* Date : 2021/01/25 logzhan
|
||||
*---------------------------------------------------------------------**/
|
||||
double gpsYaw2GoogleYaw(double heading) {
|
||||
double gh = R2D(heading) - 180;
|
||||
while (gh > 360)gh -= 360;
|
||||
while (gh < 0)gh += 360;
|
||||
return gh;
|
||||
}
|
||||
|
||||
/**----------------------------------------------------------------------
|
||||
* Function : updateResTrack
|
||||
* Description : 更新新输出的结果轨迹，包含GPS轨迹和PDR轨迹
|
||||
* Date : 2021/01/25 logzhan
|
||||
*---------------------------------------------------------------------**/
|
||||
void updateResTrack(ResultTracks& resTrack, lct_fs& lctfs)
|
||||
{
|
||||
resTrack.pdrTrack[resTrack.pdrLen].lat = lctfs.latitude;
|
||||
resTrack.pdrTrack[resTrack.pdrLen].lon = lctfs.longitudinal;
|
||||
resTrack.pdrTrack[resTrack.pdrLen].heading =
|
||||
gpsYaw2GoogleYaw(lctfs.pdrHeading);
|
||||
resTrack.pdrTrack[resTrack.pdrLen].hdop = lctfs.hdop;
|
||||
resTrack.pdrTrack[resTrack.pdrLen].accuracy = lctfs.accuracy;
|
||||
resTrack.pdrTrack[resTrack.pdrLen].time = lctfs.time;
|
||||
resTrack.pdrTrack[resTrack.pdrLen].motionType = lctfs.motionType;
|
||||
resTrack.pdrLen++;
|
||||
|
||||
if (lctfs.gpsLat > 0 && lctfs.gpsLon > 0) {
|
||||
resTrack.gpsTrack[resTrack.gpsLen].lat = lctfs.gpsLat;
|
||||
resTrack.gpsTrack[resTrack.gpsLen].lon = lctfs.gpsLon;
|
||||
resTrack.gpsTrack[resTrack.gpsLen].heading =
|
||||
gpsYaw2GoogleYaw(lctfs.gpsHeading);
|
||||
|
||||
resTrack.gpsTrack[resTrack.gpsLen].vel = lctfs.gpsSpeed;
|
||||
resTrack.gpsTrack[resTrack.gpsLen].hdop = lctfs.hdop;
|
||||
resTrack.gpsTrack[resTrack.gpsLen].accuracy = lctfs.accuracy;
|
||||
resTrack.gpsTrack[resTrack.gpsLen].time = lctfs.time;
|
||||
resTrack.gpsLen++;
|
||||
}
|
||||
}
|
||||
|
||||
string time2str(double t) {
|
||||
long time = (long)t;
|
||||
if (t < 10) {
|
||||
string str = "0" + std::to_string(time);
|
||||
return str;
|
||||
}else {
|
||||
string str = std::to_string(time);
|
||||
return str;
|
||||
}
|
||||
}
|
||||
|
||||
/**----------------------------------------------------------------------
|
||||
* Function : KmlWrite
|
||||
* Description : 将pdr算法输出的gps和pdr轨迹写为kml形式
|
||||
* path : kml文件的输出文件路径
|
||||
* name : kml文件主体名称
|
||||
* postfix ：在主体名称后面添加的后缀，用于区分类型或者版本
|
||||
* Date : 2020/11/1 logzhan
|
||||
*---------------------------------------------------------------------**/
|
||||
void KmlWrite(string path, string name, string postfix)
|
||||
{
|
||||
string pdrColor = "ff0000ff";
|
||||
string gpsColor = "ff00ffff";
|
||||
|
||||
if (path == "") {
|
||||
return;
|
||||
}
|
||||
|
||||
string kmlPath = path + name + postfix + ".kml";
|
||||
string kmlName = name + postfix;
|
||||
FILE* fid = fopen(kmlPath.c_str(), "w");
|
||||
|
||||
fprintf(fid, "<?xml version=\"1.0\" encoding=\"utf-8\"?>\n");
|
||||
fprintf(fid, "<kml xmlns=\"http://www.opengis.net/kml/2.2\">\n");
|
||||
fprintf(fid, "\t<Document>\n");
|
||||
fprintf(fid, "\t\t<name>%s</name>\n", kmlName.c_str());
|
||||
|
||||
fprintf(fid, "\t\t<Folder id=\"ID1\">\n");
|
||||
fprintf(fid, "\t\t<name>GPS Fixs</name>\n");
|
||||
|
||||
// 写gps结果
|
||||
for (int i = 0; i < resTracks.gpsLen; i++) {
|
||||
fprintf(fid, "\t\t<Placemark>\n");
|
||||
fprintf(fid, "\t\t\t<Style>\n");
|
||||
fprintf(fid, "\t\t\t\t<IconStyle>\n");
|
||||
fprintf(fid, "\t\t\t\t\t<Icon>\n");
|
||||
fprintf(fid, "\t\t\t\t\t\t<href>%s</href>\n",
|
||||
"http://maps.google.com/mapfiles/kml/shapes/arrow.png");
|
||||
fprintf(fid, "\t\t\t\t\t</Icon>\n");
|
||||
fprintf(fid, "\t\t\t\t\t<scale>%.2f</scale>\n", 0.4);
|
||||
|
||||
// 描述轨迹方向信息
|
||||
fprintf(fid, "\t\t\t\t\t<heading>%.7f</heading>\n", resTracks.gpsTrack[i].heading);
|
||||
fprintf(fid, "\t\t\t\t\t<color>%s</color>\n", gpsColor.c_str());
|
||||
fprintf(fid, "\t\t\t\t</IconStyle>\n");
|
||||
fprintf(fid, "\t\t\t</Style>\n");
|
||||
|
||||
// 写入时间信息
|
||||
double h, m, s;
|
||||
pdr_utc2hms(resTracks.gpsTrack[i].time, &h, &m, &s);
|
||||
fprintf(fid, "\t\t\t<TimeStamp>\n");
|
||||
fprintf(fid, "\t\t\t\t<when>2021-01-22T%s:%s:%s</when>\n", time2str(h).c_str(),
|
||||
time2str(m).c_str(), time2str(s).c_str());
|
||||
fprintf(fid, "\t\t\t</TimeStamp>\n");
|
||||
|
||||
// 写入描述信息
|
||||
fprintf(fid, "\t\t\t<description>\n");
|
||||
fprintf(fid, "\t\t\t\t<![CDATA[\n");
|
||||
fprintf(fid, "\t\t\t\t<dl>\n");
|
||||
fprintf(fid, "\t\t\t\t<dd>East : 0.0 (m/s)</dd>\n");
|
||||
fprintf(fid, "\t\t\t\t<dd>HDOP : %.2f (m/s)</dd>\n",
|
||||
resTracks.gpsTrack[i].hdop);
|
||||
fprintf(fid, "\t\t\t\t<dd>accuracy : %.2f (m)</dd>\n",
|
||||
resTracks.gpsTrack[i].accuracy);
|
||||
fprintf(fid, "\t\t\t\t<dd>speed : %.2f (m/s)</dd>\n",
|
||||
resTracks.gpsTrack[i].vel);
|
||||
fprintf(fid, "\t\t\t\t<dd>Heading : %.2f (degrees) </dd>\n",
|
||||
resTracks.gpsTrack[i].heading);
|
||||
fprintf(fid, "\t\t\t\t</dl><hr>]]>\n");
|
||||
fprintf(fid, "\t\t\t</description>\n");
|
||||
// 写入坐标信息
|
||||
fprintf(fid, "\t\t\t<Point>\n");
|
||||
fprintf(fid, "\t\t\t\t<coordinates>%.10f,%.10f</coordinates>\n",
|
||||
resTracks.gpsTrack[i].lon,
|
||||
resTracks.gpsTrack[i].lat);
|
||||
fprintf(fid, "\t\t\t</Point>\n");
|
||||
fprintf(fid, "\t\t</Placemark>\n");
|
||||
}
|
||||
fprintf(fid, "\t\t</Folder>\n");
|
||||
|
||||
fprintf(fid, "\t\t<Folder id=\"ID2\">\n");
|
||||
fprintf(fid, "\t\t<name>PDR Fixs</name>\n");
|
||||
// 写pdr结果
|
||||
for (int i = 0; i < resTracks.pdrLen; i++) {
|
||||
fprintf(fid, "\t\t<Placemark>\n");
|
||||
fprintf(fid, "\t\t\t<Style>\n");
|
||||
fprintf(fid, "\t\t\t\t<IconStyle>\n");
|
||||
fprintf(fid, "\t\t\t\t\t<Icon>\n");
|
||||
fprintf(fid, "\t\t\t\t\t\t<href>%s</href>\n",
|
||||
"http://maps.google.com/mapfiles/kml/shapes/arrow.png");
|
||||
fprintf(fid, "\t\t\t\t\t</Icon>\n");
|
||||
fprintf(fid, "\t\t\t\t\t<scale>%.2f</scale>\n", 0.4);
|
||||
fprintf(fid, "\t\t\t\t\t<heading>%.7f</heading>\n",
|
||||
resTracks.pdrTrack[i].heading);
|
||||
|
||||
fprintf(fid, "\t\t\t\t\t<color>%s</color>\n", pdrColor.c_str());
|
||||
fprintf(fid, "\t\t\t\t</IconStyle>\n");
|
||||
fprintf(fid, "\t\t\t</Style>\n");
|
||||
|
||||
// 写入时间信息
|
||||
double h, m, s;
|
||||
pdr_utc2hms(resTracks.pdrTrack[i].time, &h, &m, &s);
|
||||
fprintf(fid, "\t\t\t<TimeStamp>\n");
|
||||
fprintf(fid, "\t\t\t\t<when>2021-01-22T%s:%s:%s</when>\n", time2str(h).c_str(),
|
||||
time2str(m).c_str(), time2str(s).c_str());
|
||||
fprintf(fid, "\t\t\t</TimeStamp>\n");
|
||||
|
||||
// description
|
||||
fprintf(fid, "\t\t\t<description>\n");
|
||||
fprintf(fid, "\t\t\t\t<![CDATA[\n");
|
||||
fprintf(fid, "\t\t\t\t<dl>\n");
|
||||
fprintf(fid, "\t\t\t\t<dd>East : 0.0 (m/s)</dd>\n");
|
||||
fprintf(fid, "\t\t\t\t<dd>HDOP : %.2f (m/s)</dd>\n",
|
||||
resTracks.pdrTrack[i].hdop);
|
||||
|
||||
fprintf(fid, "\t\t\t\t<dd>MOTION TYPE : %s (m/s)</dd>\n",
|
||||
motionType2Str(resTracks.pdrTrack[i].motionType));
|
||||
|
||||
fprintf(fid, "\t\t\t\t<dd>accuracy : %.2f (m/s)</dd>\n",
|
||||
resTracks.pdrTrack[i].accuracy);
|
||||
fprintf(fid, "\t\t\t\t<dd>Heading : %.2f (degrees) </dd>\n",
|
||||
resTracks.pdrTrack[i].heading);
|
||||
fprintf(fid, "\t\t\t\t</dl><hr>]]>\n");
|
||||
fprintf(fid, "\t\t\t</description>\n");
|
||||
|
||||
fprintf(fid, "\t\t\t<Point>\n");
|
||||
fprintf(fid, "\t\t\t\t<coordinates>%.10f,%.10f</coordinates>\n",
|
||||
resTracks.pdrTrack[i].lon,
|
||||
resTracks.pdrTrack[i].lat);
|
||||
fprintf(fid, "\t\t\t</Point>\n");
|
||||
fprintf(fid, "\t\t</Placemark>\n");
|
||||
}
|
||||
fprintf(fid, "\t\t</Folder>\n");
|
||||
|
||||
fprintf(fid, "\t</Document>\n");
|
||||
fprintf(fid, "</kml>\n");
|
||||
|
||||
fclose(fid);
|
||||
}
|
||||
|
||||
/**----------------------------------------------------------------------
|
||||
* Function : motionType2Str
|
||||
* Description : 把用户运动类型转换为字符串输出
|
||||
* Date : 2020/8/3 logzhan
|
||||
*---------------------------------------------------------------------**/
|
||||
const char* motionType2Str(int type)
|
||||
{
|
||||
if (type == PDR_MOTION_TYPE_STATIC) {
|
||||
return "STATIC";
|
||||
}
|
||||
else if (type == PDR_MOTION_TYPE_IRREGULAR) {
|
||||
return "IRREGULAR";
|
||||
}
|
||||
else if (type == PDR_MOTION_TYPE_HANDHELD) {
|
||||
return "HANDLED";
|
||||
}
|
||||
else if (type == PDR_MOTION_TYPE_SWINGING) {
|
||||
return "SWINGING";
|
||||
}
|
||||
return "UNKOWN";
|
||||
}
|
||||
|
||||
|
||||
#ifdef _WIN32
|
||||
/**----------------------------------------------------------------------
|
||||
* Function : PDR_Init
|
||||
* Description : 初始化PDR算法(DLL调用)
|
||||
* Date : 2022/09/15 logzhan
|
||||
*---------------------------------------------------------------------**/
|
||||
void LibPDR_Init() {
|
||||
Algorithm_Init();
|
||||
}
|
||||
|
||||
/**----------------------------------------------------------------------
|
||||
* Function : pdrSumulatorUpdateGpsStep
|
||||
* Description : 仿真器按照GPS单步执行(DLL调用)
|
||||
* Date : 2020/8/3 logzhan
|
||||
*---------------------------------------------------------------------**/
|
||||
PosFusion pdrSumulatorUpdateGpsStep(int useGpsFlg)
|
||||
{
|
||||
PosFusion fusion;
|
||||
fusion.vaild = 0;
|
||||
|
||||
lct_nmea location_nmea;
|
||||
lct_nmea* ln = &location_nmea;
|
||||
imu imu_data;
|
||||
lct_fs lct_fusion;
|
||||
memset(&lct_fusion, 0, sizeof(lct_fusion));
|
||||
//memset(&location_nmea, 0, sizeof(location_nmea));
|
||||
memset(&imu_data, 0, sizeof(imu_data));
|
||||
/*memset(&kf, 0, sizeof(kf));*/
|
||||
char line[256] = { 0 };
|
||||
while (fgets(line, 256, fpSimulatorFile)) {
|
||||
|
||||
int flag = pdr_locationMainLoopStr(line, &lct_fusion);
|
||||
|
||||
if (flag) {
|
||||
|
||||
fusion.gpsLat = lct_fusion.gpsLat;
|
||||
fusion.gpsLon = lct_fusion.gpsLon;
|
||||
}
|
||||
|
||||
// 写PDR融合结果
|
||||
if (flag) {
|
||||
fusion.lat = lct_fusion.latitude;
|
||||
fusion.lon = lct_fusion.longitudinal;
|
||||
|
||||
fusion.t = lct_fusion.time;
|
||||
fusion.vaild = 1;
|
||||
|
||||
break;
|
||||
}
|
||||
}
|
||||
return fusion;
|
||||
}
|
||||
|
||||
/**----------------------------------------------------------------------
|
||||
* Function : setSimulatorFileCsvPath
|
||||
* Description : 设置仿真文件的路径(DLL调用)
|
||||
* Date : 2020/8/3 logzhan
|
||||
*---------------------------------------------------------------------**/
|
||||
void setSimulatorFileCsvPath(char* path) {
|
||||
if (fpSimulatorFile == NULL) {
|
||||
fpSimulatorFile = fopen(path, "rb");
|
||||
}
|
||||
else {
|
||||
fclose(fpSimulatorFile);
|
||||
fpSimulatorFile = NULL;
|
||||
}
|
||||
}
|
||||
|
||||
/**----------------------------------------------------------------------
|
||||
* Function : setRefGpsYaw
|
||||
* Description : 设置GPS参考角度
|
||||
* Date : 2020/8/3 logzhan
|
||||
*---------------------------------------------------------------------**/
|
||||
void setRefGpsYaw() {
|
||||
refGpsYaw = g_pdr.gpsHeading;
|
||||
printf("ref Gps Yaw = %f\n", refGpsYaw);
|
||||
//g_pdr.sysStatus = IS_INITIAL;
|
||||
//g_pdr.fusionPdrFlg = ON;
|
||||
}
|
||||
|
||||
/**----------------------------------------------------------------------
|
||||
* Function : closePdrAlgo
|
||||
* Description : 关闭pdr算法，释放相关内存，用于dll调用(DLL调用)
|
||||
* Date : 2020/8/3 logzhan
|
||||
*---------------------------------------------------------------------**/
|
||||
void closePdrAlgo() {
|
||||
pdr_closeAlgorithm();
|
||||
|
||||
if (fpSimulatorFile != NULL) {
|
||||
fclose(fpSimulatorFile);
|
||||
fpSimulatorFile = NULL;
|
||||
}
|
||||
}
|
||||
#endif
|
||||
|
|
@ -1,195 +0,0 @@
|
|||
/******************** (C) COPYRIGHT 2022 Geek************************************
|
||||
* File Name : matrix.c
|
||||
* Current Version : V2.0
|
||||
* Author : logzhan
|
||||
* Date of Issued : 2022.09.14
|
||||
* Comments : PDR矩阵运算库支持
|
||||
********************************************************************************/
|
||||
/* Header File Including -----------------------------------------------------*/
|
||||
#include "pdr_matrix.h"
|
||||
|
||||
/**---------------------------------------------------------------------
|
||||
* Function : MatrixTrans
|
||||
* Description : 矩阵转置
|
||||
* Date : 2022/09/14 logzhan
|
||||
*---------------------------------------------------------------------**/
|
||||
void MatrixTrans(double a[N][N], double r[N][N]) {
|
||||
int i, j;
|
||||
for (i = 0; i < N; i++) {
|
||||
for (j = 0; j < N; j++) {
|
||||
r[i][j] = a[j][i];
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/**---------------------------------------------------------------------
|
||||
* Function : VecMatMultiply
|
||||
* Description : 向量和矩阵相乘 r = b * a
|
||||
* Date : 2022/09/14 logzhan
|
||||
*---------------------------------------------------------------------**/
|
||||
void VecMatMultiply(double a[N], double b[N][N], double r[N]) {
|
||||
int i, j;
|
||||
double temp[N] = { 0.0 };
|
||||
//for (i = 0; i < N; i++) {
|
||||
// temp[i] = a[i];
|
||||
//}
|
||||
for (i = 0; i < N; i++) {
|
||||
for (j = 0; j < N; j++) {
|
||||
temp[i] += ((b[i][j] * 100) * (a[j] * 100)) / 10000;
|
||||
}
|
||||
}
|
||||
for (i = 0; i < N; i++) {
|
||||
r[i] = temp[i];
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
/**---------------------------------------------------------------------
|
||||
* Function : MatrixMultiply
|
||||
* Description : 矩阵和矩阵相乘 r = a * b
|
||||
* Date : 2022/09/14 logzhan
|
||||
*---------------------------------------------------------------------**/
|
||||
void MatrixMultiply(double a[N][N], double b[N][N], double r[N][N]) {
|
||||
int i, j, m;
|
||||
double temp[N][N] = { { 0.0 } };
|
||||
for (i = 0; i < N; i++) {
|
||||
for (j = 0; j < N; j++) {
|
||||
for (m = 0; m < N; m++) {
|
||||
//temp[i][j] += a[j][m] * b[m][j];
|
||||
temp[i][j] += a[i][m] * b[m][j];
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
for (i = 0; i < N; i++) {
|
||||
for (j = 0; j < N; j++) {
|
||||
r[i][j] = temp[i][j];
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/**---------------------------------------------------------------------
|
||||
* Function : MatrixAdd
|
||||
* Description : 矩阵和矩阵相加 r = a + b, 注意这个函数支持a = a + b
|
||||
* Date : 2022/09/14 logzhan
|
||||
*---------------------------------------------------------------------**/
|
||||
void MatrixAdd(double a[N][N], double b[N][N], double r[N][N]) {
|
||||
int i, j;
|
||||
for (i = 0; i < N; i++) {
|
||||
for (j = 0; j < N; j++) {
|
||||
r[i][j] = a[i][j] + b[i][j];
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
/**---------------------------------------------------------------------
|
||||
* Function : VectorAdd
|
||||
* Description : 向量和向量相加 r = a + b, 注意这个函数支持a = a + b
|
||||
* Date : 2022/09/14 logzhan
|
||||
*---------------------------------------------------------------------**/
|
||||
void VectorAdd(double a[N], double b[N], double r[N]) {
|
||||
int i;
|
||||
for (i = 0; i < N; i++) {
|
||||
r[i] = a[i] + b[i];
|
||||
}
|
||||
}
|
||||
|
||||
/**---------------------------------------------------------------------
|
||||
* Function : MatrixSub
|
||||
* Description : 矩阵和矩阵相减 r = a - b, 注意这个函数支持a = a - b
|
||||
* Date : 2022/09/14 logzhan
|
||||
*---------------------------------------------------------------------**/
|
||||
void MatrixSub(double a[N][N], double b[N][N], double r[N][N]) {
|
||||
int i, j;
|
||||
for (i = 0; i < N; i++) {
|
||||
for (j = 0; j < N; j++) {
|
||||
r[i][j] = a[i][j] - b[i][j];
|
||||
}
|
||||
}
|
||||
}
|
||||
/**---------------------------------------------------------------------
|
||||
* Function : VectorSub
|
||||
* Description : 向量和向量相减 r = a - b, 注意这个函数支持a = a - b
|
||||
* Date : 2022/09/14 logzhan
|
||||
*---------------------------------------------------------------------**/
|
||||
void VectorSub(double a[N], double b[N], double r[N]) {
|
||||
int i;
|
||||
for (i = 0; i < N; i++) {
|
||||
r[i] = a[i] - b[i];
|
||||
}
|
||||
}
|
||||
|
||||
/**---------------------------------------------------------------------
|
||||
* Function : MatrixInverse
|
||||
* Description : 求矩阵的逆矩阵
|
||||
* Date : 2022/09/14 logzhan
|
||||
*---------------------------------------------------------------------**/
|
||||
void MatrixInverse(double(*a)[N], double (*a_inv)[N]) {
|
||||
|
||||
double l[N][N] = { { 0.0 } };
|
||||
double u[N][N] = { { 0.0 } };
|
||||
double l_inv[N][N] = { { 0.0 } };
|
||||
double u_inv[N][N] = { { 0.0 } };
|
||||
double temp[N][N] = { { 0.0 } };
|
||||
|
||||
int i, j, k;
|
||||
double s;
|
||||
|
||||
for (i = 0; i < N; i++)l[i][i] = 1;
|
||||
|
||||
for (i = 0; i < N; i++){
|
||||
for (j = i; j <N; j++){
|
||||
s = 0;
|
||||
for (k = 0; k < i; k++){
|
||||
s += l[i][k] * u[k][j];
|
||||
}
|
||||
u[i][j] = a[i][j] - s;
|
||||
}
|
||||
for (j = i + 1; j < N; j++){
|
||||
s = 0;
|
||||
for (k = 0; k < i; k++){
|
||||
s += l[j][k] * u[k][i];
|
||||
}
|
||||
l[j][i] = (a[j][i] - s) / u[i][i];
|
||||
}
|
||||
}
|
||||
for (i = 0; i < N; i++)l_inv[i][i] = 1;
|
||||
|
||||
for (i = 1; i < N; i++){
|
||||
for (j = 0; j < i; j++){
|
||||
s = 0;
|
||||
for (k = 0; k < i; k++){
|
||||
s += l[i][k] * l_inv[k][j];
|
||||
}
|
||||
l_inv[i][j] = -s;
|
||||
}
|
||||
}
|
||||
for (i = 0; i < N; i++){
|
||||
u_inv[i][i] = 1 / u[i][i];
|
||||
}
|
||||
for (i = 1; i < N; i++)
|
||||
{
|
||||
for (j = i - 1; j >= 0; j--)
|
||||
{
|
||||
s = 0;
|
||||
for (k = j + 1; k <= i; k++)
|
||||
{
|
||||
s += u[j][k] * u_inv[k][i];
|
||||
}
|
||||
u_inv[j][i] = -s / u[j][j];
|
||||
}
|
||||
}
|
||||
for (i = 0; i < N; i++){
|
||||
for (j = 0; j < N; j++){
|
||||
for (k = 0; k < N; k++){
|
||||
temp[i][j] += u_inv[i][k] * l_inv[k][j];
|
||||
}
|
||||
}
|
||||
}
|
||||
for (i = 0; i < N; i++) {
|
||||
for (j = 0; j < N; j++) {
|
||||
a_inv[i][j] = temp[i][j];
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
@ -1,886 +0,0 @@
|
|||
/******************** (C) COPYRIGHT 2020 Geek************************************
|
||||
* File Name : pdr_parseData.c
|
||||
* Department : Sensor Algorithm Team
|
||||
* Current Version : V1.0
|
||||
* Author :
|
||||
* Date of Issued : 2021.01.29
|
||||
* Comments : GNSS数据解析，解析来自上层传入算法的字符串数据。主要是两种类
|
||||
型。IMU字符串和NMEA字符串。
|
||||
IMU格式 : time_ms, type, data_x, data_y, data_z
|
||||
GPS格式 ：time_ms, type, utc_time(second unit), $GPXXX,...
|
||||
其中time_ms是时间戳单位，时间单位要求是ms，不是ms需要转换为ms
|
||||
type是传感器类型码，代表传入字符串所代表的传感器类型。
|
||||
NMEA协议一般是$G开头。NMEA协议一般会附带utc时间，单位是秒(s)
|
||||
********************************************************************************/
|
||||
/* Header File Including -----------------------------------------------------*/
|
||||
#include <stdio.h>
|
||||
#include <math.h>
|
||||
#include <malloc.h>
|
||||
#include <string.h>
|
||||
#include <errno.h>
|
||||
#include <stdlib.h>
|
||||
#include <stdbool.h>
|
||||
#include "parseData.h"
|
||||
#include "pdr_sensor.h"
|
||||
#include "pdr_api.h"
|
||||
|
||||
extern sensor_agm SensorDataHist[IMU_LAST_COUNT];
|
||||
|
||||
|
||||
char * strtok_ct(char * s, const char * delim)
|
||||
{
|
||||
static char* lasts;
|
||||
if (s == 0)s = lasts;
|
||||
if (*s == '\0')return 0;
|
||||
lasts = s + strcspn(s, delim);
|
||||
if (*lasts != 0)*lasts++ = 0;
|
||||
return s;
|
||||
}
|
||||
|
||||
/**----------------------------------------------------------------------
|
||||
* Function : Int2Hex
|
||||
* Description : 十六进制数转换为十进制数
|
||||
* Date : 2020/7/9 yuanlin@vivo.com & logzhan
|
||||
*---------------------------------------------------------------------**/
|
||||
char* Int2Hex(int a, char *buffer)
|
||||
{
|
||||
sprintf(buffer, "%X", a);
|
||||
return (buffer);
|
||||
}
|
||||
|
||||
/**----------------------------------------------------------------------
|
||||
* Function : HexToDec
|
||||
* Description : 十六进制数转换为十进制数
|
||||
* Date : 2022-09-15 logzhan
|
||||
*---------------------------------------------------------------------**/
|
||||
long HexToDec(char *source)
|
||||
{
|
||||
long sum = 0, t = 1;
|
||||
int len = (int)strlen(source);
|
||||
for (int i = len - 1; i >= 0; i--) {
|
||||
sum += t * pdr_getIndexOfSigns(*(source + i));
|
||||
t *= 16;
|
||||
}
|
||||
return sum;
|
||||
}
|
||||
|
||||
/**----------------------------------------------------------------------
|
||||
* Function : pdr_getIndexOfSigns
|
||||
* Description : 返回ch字符在sign数组中的序号
|
||||
* Date : 2020/7/9 yuanlin@vivo.com &logzhan
|
||||
*---------------------------------------------------------------------**/
|
||||
int pdr_getIndexOfSigns(char ch)
|
||||
{
|
||||
if (ch >= '0' && ch <= '9')return ch - '0';
|
||||
if (ch >= 'A' && ch <= 'F')return ch - 'A' + 10;
|
||||
if (ch >= 'a' && ch <= 'f')return ch - 'a' + 10;
|
||||
return -1;
|
||||
}
|
||||
|
||||
void ParseIMU(char* pt, imu *imu_p, double ts, int sstp)
|
||||
{
|
||||
int i = 0;
|
||||
int j = 0;
|
||||
sensor_agm data_xyz = { 0 };
|
||||
sensor_agm outputData = { 0 };
|
||||
data_xyz.update = 1;
|
||||
while (pt && 3 > i)
|
||||
{
|
||||
if (0 == i)
|
||||
{
|
||||
data_xyz.s[0] = (float)atof(pt);
|
||||
}
|
||||
else if (1 == i)
|
||||
{
|
||||
data_xyz.s[1] = (float)atof(pt);
|
||||
}
|
||||
else if (2 == i)
|
||||
{
|
||||
data_xyz.s[2] = (float)atof(pt);
|
||||
}
|
||||
data_xyz.time = ts;
|
||||
|
||||
pt = strtok_ct(NULL, ",");
|
||||
i++;
|
||||
}
|
||||
|
||||
#if 0
|
||||
if (SENSOR_ACC == sstp)
|
||||
{
|
||||
memcpy(&imu_p->acc, &data_xyz, sizeof(sensor_agm));
|
||||
}
|
||||
else if (SENSOR_MAG == sstp)
|
||||
{
|
||||
memcpy(&imu_p->mag, &data_xyz, sizeof(sensor_agm));
|
||||
}
|
||||
else if (SENSOR_GYRO == sstp)
|
||||
{
|
||||
memcpy(&imu_p->gyro, &data_xyz, sizeof(sensor_agm));
|
||||
}
|
||||
#else
|
||||
data_xyz.type = sstp;
|
||||
//Hist_SensorData第0位为时间上最靠前的数据
|
||||
if (SensorDataHist[0].time > data_xyz.time)
|
||||
{
|
||||
memcpy(&outputData, &data_xyz, sizeof(sensor_agm));
|
||||
}
|
||||
else
|
||||
{
|
||||
memcpy(&outputData, &SensorDataHist[0], sizeof(sensor_agm));
|
||||
|
||||
for (i = IMU_LAST_COUNT - 1; i >= 0; i--)
|
||||
{
|
||||
if ((SensorDataHist[i].time <= data_xyz.time))
|
||||
{
|
||||
for (j = 1; j <= i ; j++)
|
||||
{
|
||||
memcpy(&SensorDataHist[j - 1], &SensorDataHist[j], sizeof(sensor_agm));
|
||||
}
|
||||
memcpy(&SensorDataHist[i], &data_xyz, sizeof(sensor_agm));
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
if (SENSOR_ACC == outputData.type)
|
||||
{
|
||||
memcpy(&imu_p->acc, &outputData, sizeof(sensor_agm));
|
||||
}
|
||||
else if (SENSOR_MAG == outputData.type)
|
||||
{
|
||||
memcpy(&imu_p->mag, &outputData, sizeof(sensor_agm));
|
||||
}
|
||||
else if (SENSOR_GYRO == outputData.type)
|
||||
{
|
||||
memcpy(&imu_p->gyr, &outputData, sizeof(sensor_agm));
|
||||
}
|
||||
|
||||
#endif
|
||||
}
|
||||
|
||||
void parseNMEA(char* pt, lct_nmea *ln, double ts)
|
||||
{
|
||||
long i, result;
|
||||
long s = 0 ;
|
||||
char buffer[3] = {'0','0','\0'};
|
||||
char* line_p = pt + strlen(pt) + 1;
|
||||
//char sumBuf[3]
|
||||
if (strchr(line_p, '*') == NULL)
|
||||
{
|
||||
return;
|
||||
}
|
||||
|
||||
for (result = line_p[1], i = 2; line_p[i] != '*'; i++){
|
||||
result ^= line_p[i];
|
||||
}
|
||||
|
||||
buffer[0] = line_p[i + 1];
|
||||
buffer[1] = line_p[i + 2];
|
||||
s = HexToDec(buffer);
|
||||
|
||||
if (result == s) { //
|
||||
pt = strtok_ct(NULL, ",");
|
||||
/*printf("pt nmea l %s %u %u\n", pt,i, strcmp(pt, GNRMC_STR));*/
|
||||
if (strstr(pt, GGA_STR))
|
||||
{
|
||||
ParseGGA(pt, ln, ts);
|
||||
}
|
||||
else if (strstr(pt, RMC_STR))
|
||||
{
|
||||
ParseRMC(pt, ln, ts);
|
||||
}
|
||||
else if (strstr(pt, GSV_STR))
|
||||
{
|
||||
parseGSV(pt, ln, ts);
|
||||
}
|
||||
else if (strstr(pt, GSA_STR))
|
||||
{
|
||||
ParseGSA(pt, ln, ts);
|
||||
}
|
||||
//minTime：应用程序接收GPS更新的最短时间（即只有超过这个时间设定，系统才可能通知我们的程序说“GPS"——单位毫秒
|
||||
if ((ln->minTime < 1000) || (ln->minTime > ts))
|
||||
{
|
||||
ln->minTime = ts;
|
||||
}
|
||||
|
||||
if ((ln->maxTime < 1000) || (ln->maxTime < ts))
|
||||
{
|
||||
ln->maxTime = ts;
|
||||
}
|
||||
}//
|
||||
else {
|
||||
printf("unpass %s", line_p);
|
||||
}
|
||||
}
|
||||
|
||||
void preprocessNMEA(lct_nmea *ln)
|
||||
{
|
||||
if (ln->gga.update && (ln->rmc[0].update || ln->rmc[1].update) && (ln->gsa[0].update || ln->gsa[1].update || ln->gsa[2].update)
|
||||
&& (ln->gsv[0].update || ln->gsv[1].update || ln->gsv[2].update))
|
||||
{
|
||||
ln->update = 1;
|
||||
}
|
||||
else
|
||||
{
|
||||
NmeaFlag_Init(ln);
|
||||
}
|
||||
}
|
||||
|
||||
void ParseGGA(char* pt, lct_nmea *ln, double ts)
|
||||
{
|
||||
if (!strcmp(pt, GNGGA_STR))
|
||||
{
|
||||
ln->gga.type = SENSOR_LOCATION_NMEA_GNGGA;
|
||||
ln->gga.update = 1;
|
||||
}
|
||||
else if (!strcmp(pt, GPGGA_STR))
|
||||
{
|
||||
ln->gga.type = SENSOR_LOCATION_NMEA_GPGGA;
|
||||
ln->gga.update = 1;
|
||||
}
|
||||
else
|
||||
{
|
||||
return;
|
||||
}
|
||||
int i = 0;
|
||||
pt = strtok_ct(NULL, ",");
|
||||
|
||||
while (pt && i < 13)
|
||||
{
|
||||
if (pt && '\0' == *pt)
|
||||
{
|
||||
if (0 == i)
|
||||
{
|
||||
ln->gga.gga_utc = ITEM_INVALID;
|
||||
}
|
||||
else if (1 == i)
|
||||
{
|
||||
ln->gga.latitude = ITEM_INVALID;
|
||||
}
|
||||
else if (2 == i)
|
||||
{
|
||||
ln->gga.gga_latitude_ns = LATITUDE_N;
|
||||
}
|
||||
else if (3 == i)
|
||||
{
|
||||
ln->gga.longitudinal = ITEM_INVALID;
|
||||
|
||||
}
|
||||
else if (4 == i)
|
||||
{
|
||||
ln->gga.longitudinal_ew = LATITUDE_N;
|
||||
}
|
||||
else if (5 == i)
|
||||
{
|
||||
ln->gga.status = invalid;
|
||||
}
|
||||
else if (6 == i)
|
||||
{
|
||||
ln->gga.satellites_nb = ITEM_INVALID;
|
||||
}
|
||||
else if (7 == i)
|
||||
{
|
||||
ln->gga.hdop = ITEM_INVALID;
|
||||
}
|
||||
else if (8 == i)
|
||||
{
|
||||
ln->gga.altitude = ITEM_INVALID;
|
||||
}
|
||||
else if (9 == i)
|
||||
{
|
||||
ln->gga.unit_a = ITEM_INVALID;
|
||||
}
|
||||
else if (10 == i)
|
||||
{
|
||||
ln->gga.height = ITEM_INVALID;
|
||||
}
|
||||
else if (11 == i)
|
||||
{
|
||||
ln->gga.unit_h = ITEM_INVALID;
|
||||
|
||||
}
|
||||
else if (12 == i)
|
||||
{
|
||||
ln->gga.dgps = ITEM_INVALID;
|
||||
}
|
||||
else if (13 == i)
|
||||
{
|
||||
ln->gga.gga_check_sum[0] = '\0';
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
if (0 == i)
|
||||
{
|
||||
ln->gga.gga_utc = atof(pt);
|
||||
}
|
||||
else if (1 == i)
|
||||
{
|
||||
ln->gga.latitude = atof(pt);
|
||||
}
|
||||
else if (2 == i)
|
||||
{
|
||||
if (!strcmp(pt, NORTH))
|
||||
{
|
||||
ln->gga.gga_latitude_ns = LATITUDE_N;
|
||||
}
|
||||
else if (!strcmp(pt, SOUTH))
|
||||
{
|
||||
ln->gga.gga_latitude_ns = LATITUDE_S;
|
||||
}
|
||||
}
|
||||
else if (3 == i)
|
||||
{
|
||||
ln->gga.longitudinal = atof(pt);
|
||||
|
||||
}
|
||||
else if (4 == i)
|
||||
{
|
||||
if (!strcmp(pt, EAST))
|
||||
{
|
||||
ln->gga.longitudinal_ew = LONGITUDINAL_E;
|
||||
}
|
||||
else if (!strcmp(pt, WEST))
|
||||
{
|
||||
ln->gga.longitudinal_ew = LONGITUDINAL_W;
|
||||
}
|
||||
}
|
||||
else if (5 == i)
|
||||
{
|
||||
ln->gga.status = (gngga_quality)atoll(pt);
|
||||
}
|
||||
else if (6 == i)
|
||||
{
|
||||
ln->gga.satellites_nb = atoi(pt);
|
||||
}
|
||||
else if (7 == i)
|
||||
{
|
||||
ln->gga.hdop = (float)atof(pt);
|
||||
}
|
||||
else if (8 == i)
|
||||
{
|
||||
ln->gga.altitude = (float)atof(pt);
|
||||
}
|
||||
else if (9 == i)
|
||||
{
|
||||
if (!strcmp(pt, "M"))
|
||||
{
|
||||
ln->gga.unit_a = 1;
|
||||
}
|
||||
}
|
||||
else if (10 == i)
|
||||
{
|
||||
ln->gga.height = (float)atof(pt);
|
||||
}
|
||||
else if (11 == i)
|
||||
{
|
||||
if (!strcmp(pt, "M"))
|
||||
{
|
||||
ln->gga.unit_h = 1;
|
||||
}
|
||||
|
||||
}
|
||||
}
|
||||
pt = strtok_ct(NULL, ",");
|
||||
i++;
|
||||
/*printf("pt %s %u\n", pt,i);*/
|
||||
}
|
||||
ln->gga.time = ts;
|
||||
}
|
||||
|
||||
void ParseRMC(char* pt, lct_nmea *ln, double ts)
|
||||
{
|
||||
int i = 0;
|
||||
double temp = 0;
|
||||
double rmc_time = 0;
|
||||
lct_nmea_rmc *rmc = NULL;
|
||||
|
||||
if (!strcmp(pt, GNRMC_STR))
|
||||
{
|
||||
rmc = &ln->rmc[0];
|
||||
RMC_Init(rmc);
|
||||
rmc->type = SENSOR_LOCATION_NMEA_GNRMC;
|
||||
rmc->update = 1;
|
||||
}
|
||||
else if (!strcmp(pt, GPRMC_STR))
|
||||
{
|
||||
rmc = &ln->rmc[1];
|
||||
RMC_Init(rmc);
|
||||
rmc->type = SENSOR_LOCATION_NMEA_GPRMC;
|
||||
rmc->update = 1;
|
||||
}
|
||||
else
|
||||
{
|
||||
return;
|
||||
}
|
||||
|
||||
pt = strtok_ct(NULL, ",");
|
||||
|
||||
while (pt && 12>i)
|
||||
{
|
||||
if (strlen(pt) > 0)
|
||||
{
|
||||
if (0 == i)
|
||||
{
|
||||
rmc_time = atof(pt);
|
||||
rmc->rmc_utc = rmc_time;
|
||||
}
|
||||
else if (1 == i)
|
||||
{
|
||||
if (!strcmp(pt, "V"))
|
||||
{
|
||||
rmc->status = POSITIONING_N;
|
||||
}
|
||||
else if (!strcmp(pt, "A"))
|
||||
{
|
||||
rmc->status = POSITIONING_Y;
|
||||
}
|
||||
}
|
||||
else if (2 == i)
|
||||
{
|
||||
temp = atof(pt);
|
||||
rmc->latitude = (floor(temp / 100) + (temp - floor(temp / 100) * 100) / 60);
|
||||
}
|
||||
else if (3 == i)
|
||||
{
|
||||
if (!strcmp(pt, NORTH))
|
||||
{
|
||||
rmc->rmc_latitude_ns = LATITUDE_N;
|
||||
}
|
||||
else if (!strcmp(pt, SOUTH))
|
||||
{
|
||||
rmc->rmc_latitude_ns = LATITUDE_S;
|
||||
}
|
||||
}
|
||||
else if (4 == i)
|
||||
{
|
||||
temp = atof(pt);
|
||||
rmc->longitudinal = (floor(temp / 100) + (temp - floor(temp / 100) * 100) / 60);
|
||||
}
|
||||
else if (5 == i)
|
||||
{
|
||||
if (!strcmp(pt, EAST))
|
||||
{
|
||||
rmc->longitudinal_ew = LONGITUDINAL_E;
|
||||
}
|
||||
else if (!strcmp(pt, WEST))
|
||||
{
|
||||
rmc->longitudinal_ew = LONGITUDINAL_W;
|
||||
}
|
||||
}
|
||||
else if (6 == i)
|
||||
{
|
||||
rmc->speed = (float)atof(pt);
|
||||
}
|
||||
else if (7 == i)
|
||||
{
|
||||
rmc->cog = (float)atof(pt);
|
||||
}
|
||||
else if (8 == i)
|
||||
{
|
||||
rmc->utc_ddmmyy = (float)atof(pt);
|
||||
}
|
||||
else if (9 == i)
|
||||
{
|
||||
rmc->magnetic = (float)atof(pt);
|
||||
}
|
||||
else if (10 == i)
|
||||
{
|
||||
if (!strcmp(pt, EAST))
|
||||
{
|
||||
rmc->mag_h = MAG_E;
|
||||
}
|
||||
else if (!strcmp(pt, WEST))
|
||||
{
|
||||
rmc->mag_h = MAG_W;
|
||||
}
|
||||
}
|
||||
else if (11 == i)
|
||||
{
|
||||
if (!strcmp(pt, "A"))
|
||||
{
|
||||
rmc->mode = MODE_A;
|
||||
}
|
||||
else if (!strcmp(pt, "D"))
|
||||
{
|
||||
rmc->mode = MODE_D;
|
||||
}
|
||||
else if (!strcmp(pt, "E"))
|
||||
{
|
||||
rmc->mode = MODE_E;
|
||||
}
|
||||
else if (!strcmp(pt, "N"))
|
||||
{
|
||||
rmc->mode = MODE_N;
|
||||
}
|
||||
}
|
||||
else if (12 == i)
|
||||
{
|
||||
}
|
||||
}
|
||||
pt = strtok_ct(NULL, ",");
|
||||
i++;
|
||||
}
|
||||
rmc->time = ts;
|
||||
}
|
||||
|
||||
void parseGSV(char* pt, lct_nmea *ln, double ts)
|
||||
{
|
||||
char *pt_gsv = NULL;
|
||||
|
||||
//memset(&ln->gsv, ITEM_INVALID, sizeof(ln->gsv));
|
||||
int i = 0;
|
||||
lct_nmea_gsv* gsv = NULL;
|
||||
if (!strcmp(pt, GPGSV_STR))
|
||||
{
|
||||
gsv = &ln->gsv[0];
|
||||
gsv->type = SENSOR_LOCATION_NMEA_GPGSV;
|
||||
}
|
||||
else if (!strcmp(pt, GLGSV_STR))
|
||||
{
|
||||
gsv = &ln->gsv[1];
|
||||
gsv->type = SENSOR_LOCATION_NMEA_GLGSV;
|
||||
}
|
||||
else if (!strcmp(pt, GAGSV_STR))
|
||||
{
|
||||
gsv = &ln->gsv[2];
|
||||
gsv->type = SENSOR_LOCATION_NMEA_GAGSV;
|
||||
}
|
||||
else
|
||||
{
|
||||
return;
|
||||
}
|
||||
|
||||
/*printf("gsv %s %d\n", pt, gsv->type);*/
|
||||
/*printf("nmea rmc %s %u %u\n", pt, !strcmp(pt, GNRMC_STR), rmc->type);*/
|
||||
pt = strtok_ct(NULL, ",");
|
||||
while (pt && 3>i)
|
||||
{
|
||||
if (0 == i)
|
||||
{
|
||||
gsv->sentences = atoi(pt); //total
|
||||
}
|
||||
else if (1 == i)
|
||||
{
|
||||
gsv->sentence_number = atoi(pt); // Which term of
|
||||
if (1 == gsv->sentence_number)
|
||||
{
|
||||
gsv->satellite_number = 0;
|
||||
}
|
||||
}
|
||||
else if (2 == i)
|
||||
{
|
||||
gsv->satellites = atoi(pt); //total
|
||||
break;
|
||||
}
|
||||
|
||||
pt = strtok_ct(NULL, ",");
|
||||
i++;
|
||||
}
|
||||
|
||||
pt_gsv = strtok_ct(NULL, "*");
|
||||
|
||||
pt = strtok_ct(pt_gsv, ",");
|
||||
i = 0;
|
||||
|
||||
while (pt && 16>i && gsv->satellites > 0)
|
||||
{
|
||||
if (pt && '\0' == *pt)
|
||||
{
|
||||
if (0 == i % 4)
|
||||
{
|
||||
gsv->satellites_data[gsv->satellite_number].prn = ITEM_INVALID;
|
||||
}
|
||||
else if (1 == i % 4)
|
||||
{
|
||||
gsv->satellites_data[gsv->satellite_number].elevation = ITEM_INVALID;
|
||||
}
|
||||
else if (2 == i % 4)
|
||||
{
|
||||
gsv->satellites_data[gsv->satellite_number].azimuth = ITEM_INVALID;
|
||||
}
|
||||
else if (3 == i % 4)
|
||||
{
|
||||
gsv->satellites_data[gsv->satellite_number].snr = ITEM_INVALID;
|
||||
if (gsv->satellite_number < gsv->satellites) //satellite_number是第几个;satellites是总共的个数
|
||||
{
|
||||
gsv->satellite_number++;
|
||||
if (gsv->satellite_number == gsv->satellites)
|
||||
{
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
if (0 == i % 4)
|
||||
{
|
||||
gsv->satellites_data[gsv->satellite_number].prn = atoi(pt);
|
||||
if (gsv->type == SENSOR_LOCATION_NMEA_GPGSV && gsv->satellites_data[gsv->satellite_number].prn > 32)
|
||||
{
|
||||
gsv->satellites_data[gsv->satellite_number].prn = (gsv->satellites_data[gsv->satellite_number].prn % 32) + 1;
|
||||
}
|
||||
if (gsv->type == SENSOR_LOCATION_NMEA_GLGSV && gsv->satellites_data[gsv->satellite_number].prn < 65 && gsv->satellites_data[gsv->satellite_number].prn > 99)
|
||||
{
|
||||
gsv->satellites_data[gsv->satellite_number].prn = (gsv->satellites_data[gsv->satellite_number].prn % 35) + 65;
|
||||
}
|
||||
if (gsv->type == SENSOR_LOCATION_NMEA_GAGSV && gsv->satellites_data[gsv->satellite_number].prn > 36)
|
||||
{
|
||||
gsv->satellites_data[gsv->satellite_number].prn = (gsv->satellites_data[gsv->satellite_number].prn % 36) + 1;
|
||||
}
|
||||
}
|
||||
else if (1 == i % 4)
|
||||
{
|
||||
gsv->satellites_data[gsv->satellite_number].elevation = atoi(pt); //仰角
|
||||
}
|
||||
else if (2 == i % 4)
|
||||
{
|
||||
|
||||
gsv->satellites_data[gsv->satellite_number].azimuth = atoi(pt);
|
||||
}
|
||||
else if (3 == i % 4)
|
||||
{
|
||||
gsv->satellites_data[gsv->satellite_number].snr = atoi(pt);
|
||||
// printf("2.gsv->satellites_data[gsv->satellite_number]: %d %d %d %d\n", gsv->satellites_data[gsv->satellite_number].prn, gsv->satellites_data[gsv->satellite_number].elevation, gsv->satellites_data[gsv->satellite_number].azimuth, gsv->satellites_data[gsv->satellite_number].snr);
|
||||
if (gsv->satellite_number<gsv->satellites) //satellite_number是第几个;satellites是总共的个数
|
||||
{
|
||||
gsv->satellite_number++;
|
||||
if (gsv->satellite_number == gsv->satellites)
|
||||
{
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
//printf("2.gsv->satellites_data[gsv->satellite_number]: %d %d %d %d\n", gsv->satellites_data[gsv->satellite_number].prn, gsv->satellites_data[gsv->satellite_number].elevation, gsv->satellites_data[gsv->satellite_number].azimuth, gsv->satellites_data[gsv->satellite_number].snr);
|
||||
}
|
||||
|
||||
pt = strtok_ct(NULL, ",");
|
||||
i++;
|
||||
}
|
||||
if (3 == i % 4 && !pt)
|
||||
{
|
||||
gsv->satellites_data[gsv->satellite_number].snr = ITEM_INVALID;
|
||||
if (gsv->satellite_number<gsv->satellites)
|
||||
{
|
||||
gsv->satellite_number++;
|
||||
}
|
||||
}
|
||||
|
||||
if (gsv->satellites == gsv->satellite_number)
|
||||
{
|
||||
gsv->update = 1;
|
||||
//printf_nmea(gsv->type,ln,PrintLOG_file_name);
|
||||
}
|
||||
|
||||
gsv->time = ts;
|
||||
}
|
||||
|
||||
void ParseGSA(char* pt, lct_nmea *ln, double ts)
|
||||
{
|
||||
int i = 0;
|
||||
int ind = -1;
|
||||
lct_nmea_gsa gsa = {0};
|
||||
if (!strcmp(pt, GNGSA_STR))
|
||||
{
|
||||
gsa.type = SENSOR_LOCATION_NMEA_GNGSA;
|
||||
gsa.update = 1;
|
||||
}
|
||||
else if (!strcmp(pt, GPGSA_STR))
|
||||
{
|
||||
ind = 0;
|
||||
gsa.type = SENSOR_LOCATION_NMEA_GPGSA;
|
||||
gsa.update = 1;
|
||||
}
|
||||
else if (!strcmp(pt, GLGSA_STR))
|
||||
{
|
||||
ind = 1;
|
||||
gsa.type = SENSOR_LOCATION_NMEA_GLGSA;
|
||||
gsa.update = 1;
|
||||
}
|
||||
else if (!strcmp(pt, GAGSA_STR))
|
||||
{
|
||||
ind = 2;
|
||||
gsa.type = SENSOR_LOCATION_NMEA_GAGSA;
|
||||
gsa.update = 1;
|
||||
}
|
||||
else
|
||||
{
|
||||
return;
|
||||
}
|
||||
|
||||
// printf("gsa %s %d\n", pt, ln->gsa.type);
|
||||
pt = strtok_ct(NULL, ",");
|
||||
/*printf("nmea gngsa %s %u\n", pt, i);*/
|
||||
while (pt && 17>=i)
|
||||
{
|
||||
/*printf("%u gsa pt %s\n", i, pt);*/
|
||||
if (strlen(pt) > 0)
|
||||
{
|
||||
if (0 == i)
|
||||
{
|
||||
if (!strcmp(pt, "A"))
|
||||
{
|
||||
gsa.mode = POSITIONING_MODE_A;
|
||||
}
|
||||
else if (!strcmp(pt, "M"))
|
||||
{
|
||||
gsa.mode = POSITIONING_MODE_M;
|
||||
}
|
||||
}
|
||||
else if (1 == i)
|
||||
{
|
||||
gsa.p_type = (lct_type)atoll(pt);
|
||||
}
|
||||
else if (( i >= 2 ) && ( i <= 13 ))
|
||||
{
|
||||
gsa.prn[i - 2] = atoi(pt);
|
||||
|
||||
if (ind == 0 && gsa.prn[i - 2] > 32)
|
||||
{
|
||||
gsa.prn[i - 2] = (gsa.prn[i - 2] % 32)+1;
|
||||
}
|
||||
if (ind == 1 && gsa.prn[i - 2] < 65 && gsa.prn[i - 2] > 99)
|
||||
{
|
||||
gsa.prn[i - 2] = (gsa.prn[i - 2] % 35) + 65;
|
||||
}
|
||||
if (ind == 2 && gsa.prn[i - 2] > 36)
|
||||
{
|
||||
gsa.prn[i - 2] = (gsa.prn[i - 2] % 36)+1;
|
||||
}
|
||||
if (gsa.prn[i - 2] > 64)
|
||||
{
|
||||
gsa.prnflg |= (1 << (gsa.prn[i - 2] - 64));
|
||||
if (ind < 0)
|
||||
{
|
||||
ind = 1;
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
gsa.prnflg |= (1 << gsa.prn[i - 2]);
|
||||
if (ind < 0)
|
||||
{
|
||||
if (ln->gsa[0].update && ln->gsa[1].update)
|
||||
{
|
||||
ind = 2;
|
||||
}
|
||||
else
|
||||
{
|
||||
ind = 0;
|
||||
}
|
||||
}
|
||||
}
|
||||
}else if (i == 14){
|
||||
gsa.pdop = (float)atof(pt);
|
||||
}else if (i == 15){
|
||||
gsa.hdop = (float)atof(pt);
|
||||
}else if (i == 16){
|
||||
gsa.vdop = (float)atof(pt);
|
||||
}else if (17 == i)
|
||||
{
|
||||
if (ind < 0 && pt[0] == '1')
|
||||
{
|
||||
ind = 0;
|
||||
gsa.type = SENSOR_LOCATION_NMEA_GPGSA;
|
||||
}
|
||||
else if (ind < 0 && pt[0] == '2')
|
||||
{
|
||||
ind = 1;
|
||||
gsa.type = SENSOR_LOCATION_NMEA_GLGSA;
|
||||
}
|
||||
else if (ind < 0 && pt[0] == '3')
|
||||
{
|
||||
ind = 2;
|
||||
gsa.type = SENSOR_LOCATION_NMEA_GAGSA;
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
pt = strtok_ct(NULL, ",");
|
||||
i++;
|
||||
/*printf("pt %s %u\n", pt,i);*/
|
||||
}
|
||||
|
||||
gsa.time = ts;
|
||||
if (ind >=0 && ind <=2)
|
||||
{
|
||||
memcpy(&ln->gsa[ind], &gsa, sizeof(lct_nmea_gsa));
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
/**----------------------------------------------------------------------
|
||||
* Function : parseLocAccuracy
|
||||
* Description : 解析GPS的Accuracy精度参数
|
||||
* Date : 2020/7/9 yuanlin@vivo.com &logzhan
|
||||
*---------------------------------------------------------------------**/
|
||||
void parseLocAccuracy(char* pt, lct_nmea *ln, double ts)
|
||||
{
|
||||
int i = 0;
|
||||
while (pt && 8>i){
|
||||
if ((i == 0) && (!strcmp(pt, "Status"))){
|
||||
pt = strtok_ct(NULL, ",");
|
||||
ln->accuracy.status = atoi(pt);
|
||||
return;
|
||||
}
|
||||
if (i == 7){
|
||||
ln->accuracy.update = 1;
|
||||
ln->accuracy.time = ts;
|
||||
if ((pt && '\0' == *pt))
|
||||
{
|
||||
ln->accuracy.err = -1;
|
||||
}
|
||||
else if (ln->accuracy.status == -1 || ln->accuracy.status == 2)
|
||||
{
|
||||
ln->accuracy.err = (float)atof(pt);
|
||||
}
|
||||
return;
|
||||
}
|
||||
pt = strtok_ct(NULL, ",");
|
||||
i++;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
int pdr_parseLineStr(char* line, imu* imu_p, lct_nmea* ln)
|
||||
{
|
||||
char* pt = NULL;
|
||||
int i = 0, sstp = -1;
|
||||
double ts = 0;
|
||||
pt = strtok_ct(line, ",");
|
||||
|
||||
while (pt && i < 2){
|
||||
if (i == 0)ts = atoll(pt) * 0.000001;
|
||||
else if (i == 1)sstp = atoi(pt);
|
||||
pt = strtok_ct(NULL, ",");
|
||||
i++;
|
||||
}
|
||||
switch (sstp)
|
||||
{
|
||||
case SENSOR_ACC:
|
||||
case SENSOR_MAG:
|
||||
case SENSOR_GYRO:
|
||||
ParseIMU(pt, imu_p, ts, sstp);
|
||||
break;
|
||||
case SENSOR_LOCATION_STATELLITE:
|
||||
break;
|
||||
case SENSOR_LOCATION_NETWORK:
|
||||
break;
|
||||
case SENSOR_LOCATION_NMEA:
|
||||
parseNMEA(pt, ln, ts);
|
||||
if ((ln->maxTime - ln->minTime) > NAEA_LAST_TIME)
|
||||
{
|
||||
preprocessNMEA(ln);
|
||||
}
|
||||
break;
|
||||
case SENSOR_LOCATION_FUSION:
|
||||
parseLocAccuracy(pt, ln, ts);
|
||||
break;
|
||||
}
|
||||
if ((ln->minTime > 0) && (ts - ln->minTime) > NAEA_LAST_TIME) {
|
||||
preprocessNMEA(ln);
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
|
|
@ -1,602 +0,0 @@
|
|||
/******************** (C) COPYRIGHT 2020 Geek************************************
|
||||
* File Name : pdr_util.c
|
||||
* Department : Sensor Algorithm Team
|
||||
* Current Version : V1.2
|
||||
* Author : logzhan
|
||||
* Date of Issued : 2020.7.4
|
||||
* Comments : PDR 工具类函数支持
|
||||
********************************************************************************/
|
||||
/* Header File Including -----------------------------------------------------*/
|
||||
#include <stdio.h>
|
||||
#include <math.h>
|
||||
#include <float.h>
|
||||
#include "pdr_base.h"
|
||||
#include "pdr_util.h"
|
||||
|
||||
const float FLT_THRES = 0.000001f; // 浮点数最小阈值
|
||||
|
||||
/* Function Declaration ------------------------------------------------------*/
|
||||
/**----------------------------------------------------------------------
|
||||
* Function : mean
|
||||
* Description : 对输入double数组求均值
|
||||
* Review : 求均值过程每次都做除法，有优化空间，可以针对求解均值的数值大小采用
|
||||
* 不同函数
|
||||
* Date : 2020/7/4 logzhan
|
||||
*---------------------------------------------------------------------**/
|
||||
double mean(double *data, int len) {
|
||||
int i;
|
||||
double value = 0;
|
||||
for (i = 0; i < len; i++) {
|
||||
value += data[i] / len;
|
||||
}
|
||||
return value;
|
||||
}
|
||||
/**----------------------------------------------------------------------
|
||||
* Function : fmean
|
||||
* Description : 对输入float数组求均值
|
||||
* Review : 求均值过程每次都做除法，有优化空间，可以针对求解均值的数值大小采用
|
||||
* 不同函数
|
||||
* Date : 2020/7/4 logzhan
|
||||
*---------------------------------------------------------------------**/
|
||||
float fmean(float *data, int len)
|
||||
{
|
||||
float value = 0;
|
||||
for (int i = 0; i < len; i++){
|
||||
value += data[i] / len;
|
||||
}
|
||||
return value;
|
||||
}
|
||||
|
||||
double meanAngle(double* angle, int len)
|
||||
{
|
||||
double baseAngle = angle[0];
|
||||
double diff = 0.0;
|
||||
double sum = 0.0;
|
||||
double value = 0;
|
||||
|
||||
if (len <= 0)return 0;
|
||||
|
||||
for (int i = 0; i < len; i++)
|
||||
{
|
||||
diff = angle[i] - baseAngle;
|
||||
if (diff > PI)
|
||||
{
|
||||
diff = diff - TWO_PI;
|
||||
}
|
||||
else if (diff < -PI)
|
||||
{
|
||||
diff = diff + TWO_PI;
|
||||
}
|
||||
|
||||
sum += (baseAngle + diff);
|
||||
}
|
||||
|
||||
value = sum / len;
|
||||
|
||||
modAngle(&value, 0, TWO_PI);
|
||||
return value;
|
||||
}
|
||||
|
||||
void modAngle(double * angle, double min, double max)
|
||||
{
|
||||
double value = *angle;
|
||||
double range = max - min;
|
||||
while (value > max)value = value - range;
|
||||
while (value < min)value = value + range;
|
||||
*angle = value;
|
||||
}
|
||||
|
||||
float stdf(float* data, int len)
|
||||
{
|
||||
int i;
|
||||
float meanValue = fmean(data, len);
|
||||
float value = 0;
|
||||
for (i = 0; i < len; i++)
|
||||
{
|
||||
value += (data[i] - meanValue) * (data[i] - meanValue) / len;
|
||||
}
|
||||
return (float)sqrt(value);
|
||||
}
|
||||
|
||||
/**----------------------------------------------------------------------
|
||||
* Function : pdr_invSqrt
|
||||
* Description : 计算1/sqrt(x)的快速的算法
|
||||
* Date : 2020/6/30 logzhan
|
||||
*---------------------------------------------------------------------**/
|
||||
float InvSqrt(float x) {
|
||||
float xHalf = 0.5f * x;
|
||||
int i = *(int *)&x;
|
||||
i = 0x5f3759df - (i >> 1);
|
||||
x = *(float *)&i;
|
||||
x = x * (1.5f - xHalf * x * x);
|
||||
return x;
|
||||
}
|
||||
|
||||
/**---------------------------------------------------------------------
|
||||
* Function : pdr_v3Norm
|
||||
* Description : 三维浮点数向量归一化
|
||||
* Date : 2021/01/26 yuanlin@vivocom &&
|
||||
*
|
||||
*---------------------------------------------------------------------**/
|
||||
void pdr_v3Norm(float* vx, float* vy, float* vz)
|
||||
{
|
||||
float norm = sqrtf(((*vx) * (*vx) + (*vy) * (*vy) +
|
||||
(*vz) * (*vz)));
|
||||
// 防止出现模为0的情况
|
||||
if (norm > FLT_THRES) {
|
||||
norm = 1 / norm;
|
||||
(*vx) *= norm; (*vy) *= norm; (*vz) *= norm;
|
||||
}
|
||||
}
|
||||
|
||||
/**---------------------------------------------------------------------
|
||||
* Function : quatNorm
|
||||
* Description : 四元数归一化
|
||||
* Date : 2021/01/26 yuanlin@vivocom && logzhan
|
||||
* 2021/01/27 logzhan : 优化了代码逻辑，防止当四元数
|
||||
* 全为0时产生的异常情况。
|
||||
*---------------------------------------------------------------------**/
|
||||
void QuatNorm(float* q0, float* q1, float* q2, float* q3) {
|
||||
|
||||
float norm = sqrtf(((*q0) * (*q0) + (*q1) * (*q1) +
|
||||
(*q2) * (*q2) + (*q3) * (*q3)));
|
||||
if (norm > FLT_THRES) {
|
||||
norm = 1 / norm;
|
||||
(*q0) *= norm; (*q1) *= norm;
|
||||
(*q2) *= norm; (*q3) *= norm;
|
||||
}
|
||||
}
|
||||
|
||||
double pow_i(double num, long long n)
|
||||
{
|
||||
double powint = 1;
|
||||
long long i;
|
||||
for (i = 1; i <= n; i++)
|
||||
{
|
||||
powint *= num;
|
||||
}
|
||||
return powint;
|
||||
}
|
||||
double pow_f(double num, double m)
|
||||
{
|
||||
int i, j;
|
||||
double powf = 0, x, tmpm = 1;
|
||||
x = num - 1;
|
||||
for (i = 1; tmpm>1e-12 || tmpm<-1e-12; i++)
|
||||
{
|
||||
for (j = 1, tmpm = 1; j <= i; j++)
|
||||
{
|
||||
tmpm *= (m - j + 1)*x / j;
|
||||
}
|
||||
powf += tmpm;
|
||||
}
|
||||
return powf + 1;
|
||||
}
|
||||
|
||||
// https://blog.csdn.net/xiaoxiongli/article/details/2134626
|
||||
double vivopow(double num, double m)
|
||||
{
|
||||
if (num == 0 && m != 0)
|
||||
{
|
||||
return 0;
|
||||
}
|
||||
else if (num == 0 && m == 0)
|
||||
{
|
||||
return 1;
|
||||
}
|
||||
else if (num < 0 && (m != (long long)m))
|
||||
{
|
||||
return (num - num) / (num - num);
|
||||
}
|
||||
|
||||
if (num > 2)
|
||||
{
|
||||
num = 1 / num;
|
||||
m = -m;
|
||||
}
|
||||
|
||||
if (m < 0)
|
||||
{
|
||||
return 1 / vivopow(num, -m);
|
||||
}
|
||||
|
||||
|
||||
if (m == (long long)m)
|
||||
{
|
||||
return pow_i(num, (long long)m);
|
||||
}
|
||||
|
||||
else
|
||||
{
|
||||
return pow_f(num, m - (long long)m) * pow_i(num, (long long)m);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
/**----------------------------------------------------------------------
|
||||
* Function : qnb2att
|
||||
* Description : 四元数转欧拉角
|
||||
* Date : 2020/7/4 logzhan
|
||||
*---------------------------------------------------------------------**/
|
||||
void Qnb2Att(float* q, double* attitude)
|
||||
{
|
||||
double q11 = q[0] * q[0]; double q13 = q[0] * q[2];
|
||||
double q12 = q[0] * q[1]; double q14 = q[0] * q[3];
|
||||
double q22 = q[1] * q[1]; double q24 = q[1] * q[3];
|
||||
double q23 = q[1] * q[2]; double q33 = q[2] * q[2];
|
||||
double q34 = q[2] * q[3]; double q44 = q[3] * q[3];
|
||||
|
||||
if (fabs(q34 + q12) <= 0.5) {
|
||||
attitude[0] = asin(2 * (q34 + q12));
|
||||
}
|
||||
attitude[1] = atan2(-2 * (q24 - q13), q11 - q22 - q33 + q44);
|
||||
attitude[2] = atan2(-2 * (q23 - q14), q11 - q22 + q33 - q44);
|
||||
}
|
||||
|
||||
|
||||
void QuaternConj(float _q[], float q[]) {
|
||||
_q[0] = q[0];
|
||||
_q[1] = -q[1];
|
||||
_q[2] = -q[2];
|
||||
_q[3] = -q[3];
|
||||
}
|
||||
|
||||
void quaternProd(float ab[], float a[], float b[]) {
|
||||
ab[0] = a[0] * b[0] - a[1] * b[1] - a[2] * b[2] - a[3] * b[3];
|
||||
ab[1] = a[0] * b[1] + a[1] * b[0] + a[2] * b[3] - a[3] * b[2];
|
||||
ab[2] = a[0] * b[2] - a[1] * b[3] + a[2] * b[0] + a[3] * b[1];
|
||||
ab[3] = a[0] * b[3] + a[1] * b[2] - a[2] * b[1] + a[3] * b[0];
|
||||
}
|
||||
|
||||
|
||||
void WGS842ECEF(double *plla, double *ecef) {
|
||||
double f = 1 / 298.257223563;
|
||||
double a = 6378136.49;
|
||||
double b, e, roc;
|
||||
|
||||
b = a * (1 - f);
|
||||
e = sqrt(vivopow(a, 2) - vivopow(b, 2)) / a;
|
||||
roc = a / sqrt(1 - vivopow(e*sin(plla[0]), 2));
|
||||
ecef[0] = (roc + plla[2]) * cos(plla[0]) * cos(plla[1]);
|
||||
ecef[1] = (roc + plla[2]) * cos(plla[0]) * sin(plla[1]);
|
||||
ecef[2] = (roc * (1 - e * e) + plla[2]) * sin(plla[0]);
|
||||
|
||||
}
|
||||
|
||||
void ECEF2WGS84(double *ecef, double *plla) {
|
||||
double a = 6378137;
|
||||
double b = 6356752.314245179;
|
||||
double e, ep, p, th, roc;
|
||||
|
||||
e = sqrt(vivopow(a, 2) - vivopow(b, 2)) / a;
|
||||
b = sqrt(vivopow(a, 2)*(1 - vivopow(e, 2)));
|
||||
ep = sqrt((vivopow(a, 2) - vivopow(b, 2)) / vivopow(b, 2));
|
||||
p = sqrt(ecef[0] * ecef[0] + ecef[1] * ecef[1]);
|
||||
th = atan2(a*ecef[2], b*p);
|
||||
|
||||
plla[1] = atan2(ecef[1], ecef[0]);
|
||||
plla[0] = atan2((ecef[2] + vivopow(ep, 2)*b*vivopow(sin(th), 3)), (p - vivopow(e, 2)*a*vivopow(cos(th), 3)));
|
||||
roc = a / sqrt(1 - vivopow(e*sin(plla[0]), 2));
|
||||
plla[2] = p / cos(plla[0]) - roc;
|
||||
|
||||
}
|
||||
|
||||
void ECEF2Ned(double *ecef, double *plla, double *ned) {
|
||||
ned[0] = -sin(plla[0])*cos(plla[1])*ecef[0] - sin(plla[0])*sin(plla[1])*ecef[1] + cos(plla[0])*ecef[2];
|
||||
ned[1] = -sin(plla[1])* ecef[0] + cos(plla[1])*ecef[1];
|
||||
ned[2] = -cos(plla[0])*cos(plla[1])*ecef[0] - cos(plla[0])*sin(plla[1])*ecef[1] - sin(plla[0])*ecef[2];
|
||||
}
|
||||
|
||||
void NED2ECEF(double *plla, double *ned, double *ecef0, double *ecef) {
|
||||
ecef[0] = -sin(plla[0])*cos(plla[1])*ned[0] - sin(plla[1])*ned[1] - cos(plla[0])*cos(plla[1])*ned[2] + ecef0[0];
|
||||
ecef[1] = -sin(plla[0])*sin(plla[1])*ned[0] + cos(plla[1])*ned[1] - cos(plla[0])*sin(plla[1])*ned[2] + ecef0[1];
|
||||
ecef[2] = cos(plla[0])*ned[0] - sin(plla[0])*ned[2] + ecef0[2];;
|
||||
}
|
||||
|
||||
void wgs842Ned(double *plla, double *ref_lla, double *ned)
|
||||
{
|
||||
double ecef[3] = {0};
|
||||
|
||||
WGS842ECEF(plla, ecef);
|
||||
ECEF2Ned(ecef, ref_lla, ned);
|
||||
}
|
||||
|
||||
void ned2Wgs84(double *ref_plla, double *ned, double* plla)
|
||||
{
|
||||
double ecef[3] = { 0 };
|
||||
double ref_ecef[3] = { 0 };
|
||||
WGS842ECEF(ref_plla, ref_ecef);
|
||||
|
||||
NED2ECEF(ref_plla, ned, ref_ecef, ecef);
|
||||
ECEF2WGS84(ecef, plla);
|
||||
}
|
||||
|
||||
|
||||
void getCovMatrix(double *x, double *y, double cov[2][2], int n) {
|
||||
int i;
|
||||
double sum_x = 0, sum_y = 0, sum_xy = 0;
|
||||
double m_x = 0, m_y = 0;
|
||||
m_x = mean(x, n);
|
||||
m_y = mean(y, n);
|
||||
for (i = 0; i < n; i++) {
|
||||
sum_xy += (x[i] - m_x)*(y[i] - m_y);
|
||||
sum_x += (x[i] - m_x)*(x[i] - m_x);
|
||||
sum_y += (y[i] - m_y)*(y[i] - m_y);
|
||||
}
|
||||
cov[0][0] = sum_x / n;
|
||||
cov[0][1] = sum_xy / n;
|
||||
cov[1][0] = sum_xy / n;
|
||||
cov[1][1] = sum_y / n;
|
||||
}
|
||||
|
||||
int Jacobi(double a[][2], double p[][2], int n, double eps, int T)
|
||||
{
|
||||
int i, j, k;
|
||||
double max = a[0][1];
|
||||
int row = 0;
|
||||
int col = 0;
|
||||
int ite_num = 0;
|
||||
double theta = 0;
|
||||
double aii = 0;
|
||||
double ajj = 0;
|
||||
double aij = 0;
|
||||
double sin_theta = 0;
|
||||
double cos_theta = 0;
|
||||
double sin_2theta = 0;
|
||||
double cos_2theta = 0;
|
||||
double arowk = 0;
|
||||
double acolk = 0;
|
||||
double pki = 0;
|
||||
double pkj = 0;
|
||||
|
||||
for (i = 0; i < n; i++)
|
||||
p[i][i] = 1;
|
||||
while (1)
|
||||
{
|
||||
max = fabs(a[0][1]);
|
||||
row = 0;
|
||||
col = 1;
|
||||
for (i = 0; i < n; i++)
|
||||
for (j = 0; j < n; j++)
|
||||
if (i != j && fabs(a[i][j])>max)
|
||||
{
|
||||
max = fabs(a[i][j]);
|
||||
row = i;
|
||||
col = j;
|
||||
}
|
||||
if (max < eps)
|
||||
{
|
||||
//printf("max : %lf \n", max);
|
||||
//printf("t : %lf \n", ite_num);
|
||||
|
||||
return 1;
|
||||
}
|
||||
|
||||
if (ite_num>T)
|
||||
{
|
||||
//printf("max : %lf \n", max);
|
||||
//printf("t : %lf \n", ite_num);
|
||||
return 0;
|
||||
}
|
||||
|
||||
if (a[row][row] == a[col][col])
|
||||
theta = PI / 4;
|
||||
else
|
||||
theta = 0.5*atan(2 * a[row][col] / (a[row][row] - a[col][col]));
|
||||
aii = a[row][row];
|
||||
ajj = a[col][col];
|
||||
aij = a[row][col];
|
||||
sin_theta = sin(theta);
|
||||
cos_theta = cos(theta);
|
||||
sin_2theta = sin(2 * theta);
|
||||
cos_2theta = cos(2 * theta);
|
||||
a[row][row] = aii * cos_theta*cos_theta + ajj * sin_theta*sin_theta + aij * sin_2theta;
|
||||
a[col][col] = aii * sin_theta*sin_theta + ajj * cos_theta*cos_theta - aij * sin_2theta;
|
||||
a[row][col] = 0.5*(ajj - aii)*sin_2theta + aij * cos_2theta;
|
||||
a[col][row] = a[row][col];
|
||||
for (k = 0; k < n; k++)
|
||||
{
|
||||
if (k != row && k != col)
|
||||
{
|
||||
arowk = a[row][k];
|
||||
acolk = a[col][k];
|
||||
a[row][k] = arowk * cos_theta + acolk * sin_theta;
|
||||
a[k][row] = a[row][k];
|
||||
a[col][k] = acolk * cos_theta - arowk * sin_theta;
|
||||
a[k][col] = a[col][k];
|
||||
}
|
||||
}
|
||||
if (ite_num == 0)
|
||||
{
|
||||
p[row][row] = cos_theta;
|
||||
p[row][col] = -sin_theta;
|
||||
p[col][row] = sin_theta;
|
||||
p[col][col] = cos_theta;
|
||||
}
|
||||
else
|
||||
{
|
||||
for (k = 0; k < n; k++)
|
||||
{
|
||||
pki = p[k][row];
|
||||
pkj = p[k][col];
|
||||
p[k][row] = pki * cos_theta + pkj * sin_theta;
|
||||
p[k][col] = pkj * cos_theta - pki * sin_theta;
|
||||
}
|
||||
}
|
||||
ite_num++;
|
||||
}
|
||||
}
|
||||
|
||||
static int isRealLat(double lat){ return (lat < 90) && (lat > -90); }
|
||||
static int isRealLon(double lon){ return (lon < 180) && (lon > -180); }
|
||||
|
||||
/*
|
||||
function: 计算地图上两个由经纬度表示点间的直线距离。
|
||||
param @ pointA : 点A的经纬度
|
||||
param @ pointB : 点B的经纬度
|
||||
return @ dis : A、B点间的距离；若小于0则说明A或B点的经纬度输入有误。
|
||||
*/
|
||||
double calDistance(LatLng pointA, LatLng pointB)
|
||||
{
|
||||
double dis = -1;
|
||||
double lla1[3] = { 0 };
|
||||
double lla2[3] = { 0 };
|
||||
double ned1[3] = { 0 };
|
||||
double ned2[3] = { 0 };
|
||||
double diff_ned[3] = { 0 };
|
||||
|
||||
lla1[0] = pointA.lat * RADIAN_PER_DEG;
|
||||
lla1[1] = pointA.lon * RADIAN_PER_DEG;
|
||||
lla2[0] = pointB.lat * RADIAN_PER_DEG;
|
||||
lla2[1] = pointB.lon * RADIAN_PER_DEG;
|
||||
|
||||
if (isRealLat(pointA.lat) && isRealLon(pointA.lon)
|
||||
&& isRealLat(pointB.lat) && isRealLon(pointB.lon))
|
||||
{
|
||||
wgs842Ned(lla1, lla1, ned1);
|
||||
wgs842Ned(lla2, lla1, ned2);
|
||||
diff_ned[0] = ned1[0] - ned2[0];
|
||||
diff_ned[1] = ned1[1] - ned2[1];
|
||||
diff_ned[2] = ned1[2] - ned2[2];
|
||||
|
||||
dis = sqrt(pow(diff_ned[0], 2) + pow(diff_ned[1], 2) + pow(diff_ned[2], 2));
|
||||
}
|
||||
return dis;
|
||||
}
|
||||
|
||||
|
||||
|
||||
LatLng ProjPointOfLatLng(LatLng point, LatLng linePointA, LatLng linePointB)
|
||||
{
|
||||
|
||||
/* local variables declaration */
|
||||
double gradxB_P; /* linePointB 向 point 的 x 梯度 */
|
||||
double gradxB_A; /* linePointA 向 linePointB 的 x 梯度 */
|
||||
double gradyB_A; /* linePointA 向 linePointB 的 y 梯度 */
|
||||
double normPow; /* linePointA 向 linePointB 梯度的平方和 */
|
||||
double scalarMultip; /* gradxB_A * gradyB_A 的标量乘法 */
|
||||
LatLng resProjPoint; /* 结果点坐标 */
|
||||
|
||||
/* block process */
|
||||
gradxB_A = linePointA.lat - linePointB.lat;
|
||||
gradyB_A = linePointA.lon - linePointB.lon;
|
||||
if ((fabs(gradxB_A) < DBL_EPSILON) && (fabs(gradyB_A) < DBL_EPSILON))
|
||||
{
|
||||
/* 当linePointA 和linePointB是同一点时，返回的point坐标 */
|
||||
resProjPoint.lat = point.lat;
|
||||
resProjPoint.lon = point.lon;
|
||||
}
|
||||
else
|
||||
{
|
||||
gradxB_P = point.lat - linePointB.lat;
|
||||
normPow = vivopow(gradxB_A, 2) + vivopow(gradyB_A, 2);
|
||||
resProjPoint.lon = (double)(
|
||||
(gradxB_P * gradxB_A * gradyB_A + point.lon * vivopow(gradyB_A, 2) + linePointB.lon * vivopow(gradxB_A, 2)) / normPow);
|
||||
if ((fabs(gradxB_A) < DBL_EPSILON) || (fabs(gradyB_A) < DBL_EPSILON))
|
||||
{
|
||||
/* 当linePointA 和 linePointB 所在直线与 x轴或y轴平行时做规避*/
|
||||
scalarMultip = (fabs(gradxB_A) < DBL_EPSILON) ? gradyB_A : gradxB_A;
|
||||
}
|
||||
else
|
||||
{
|
||||
scalarMultip = gradxB_A * gradyB_A;
|
||||
}
|
||||
resProjPoint.lat = (double)(
|
||||
(gradxB_A * linePointB.lat * gradyB_A + gradxB_A * gradxB_A * (resProjPoint.lon - linePointB.lon)) / scalarMultip);
|
||||
if (fabs(gradxB_A) < DBL_EPSILON)
|
||||
{
|
||||
resProjPoint.lat = linePointA.lat;
|
||||
}
|
||||
if (fabs(gradyB_A) < DBL_EPSILON)
|
||||
{
|
||||
resProjPoint.lon = point.lon;
|
||||
}
|
||||
}
|
||||
|
||||
return resProjPoint;
|
||||
}
|
||||
|
||||
/**----------------------------------------------------------------------
|
||||
* Function : CalEarthParameter
|
||||
* Description : 根据输入纬度返回该纬度地球相关参数
|
||||
* Date : 2020/7/8 logzhan
|
||||
*---------------------------------------------------------------------**/
|
||||
EarthData_t CalEarthParameter(double lat)
|
||||
{
|
||||
double temp_value = 0;
|
||||
EarthData_t earthData = { 0 };
|
||||
|
||||
if (fabs(lat) >= PI / 2)lat = 0.0f;
|
||||
|
||||
temp_value = sqrt(1 - WGS84_ECCENTR2 * (sin(lat)*sin(lat)));
|
||||
|
||||
earthData.lat = lat;
|
||||
earthData.rmh = WGS84_RE * (1 - WGS84_ECCENTR2) / (temp_value*temp_value*temp_value);
|
||||
earthData.rnh = WGS84_RE * cos(lat) / temp_value;
|
||||
return earthData;
|
||||
}
|
||||
|
||||
/**----------------------------------------------------------------------
|
||||
* Function : pdr_CalRadianDifferent
|
||||
* Description : 计算弧度之差
|
||||
* Date : 2020/7/8 logzhan
|
||||
*---------------------------------------------------------------------**/
|
||||
double CalRadianDifferent(double s_dir, double d_dir)
|
||||
{
|
||||
double dirDiff = d_dir - s_dir;
|
||||
if (fabs(dirDiff) < PI)return dirDiff;
|
||||
return dirDiff > 0 ? dirDiff - 2 * PI : dirDiff + 2 * PI;
|
||||
}
|
||||
|
||||
void ProjPointOfLatLng_cir(double point1[], double yaw, double point2[], double result[])
|
||||
{
|
||||
if (fabs(fabs(yaw) - PI / 2) < DOUBLE_ZERO)
|
||||
{
|
||||
result[0] = point1[0];
|
||||
result[1] = point2[1];
|
||||
return;
|
||||
}
|
||||
|
||||
EarthData_t earthData = CalEarthParameter(point1[0]);
|
||||
double k = -tan(yaw);
|
||||
double lonDiff = (point2[1] - point1[1]) * earthData.rnh;
|
||||
double latDiff = (point2[0] - point1[0]) * earthData.rmh;
|
||||
|
||||
double x = (k*latDiff + k * k * lonDiff) / (1 + k * k);
|
||||
double y = (latDiff + k * lonDiff) / (1 + k * k);
|
||||
|
||||
result[1] = point1[1] + x / earthData.rnh;
|
||||
result[0] = point1[0] + y / earthData.rmh;
|
||||
}
|
||||
|
||||
/**----------------------------------------------------------------------
|
||||
* Function : pdr_WriteCsvStr
|
||||
* Description : 将字符串信息写入文本，主要是对fprintf封装一层
|
||||
* Date : 2020/7/8 logzhan
|
||||
*---------------------------------------------------------------------**/
|
||||
void pdr_WriteCsvStr(FILE* fp_write, char* str)
|
||||
{
|
||||
if (NULL == fp_write)return;
|
||||
fprintf(fp_write, "%s\n", str);
|
||||
}
|
||||
|
||||
int pdr_min(int a, int b){
|
||||
return (a > b ? b : a);
|
||||
}
|
||||
|
||||
/**----------------------------------------------------------------------
|
||||
* Function : pdr_utc2hms
|
||||
* Description : 将UTC时间转换为时分秒
|
||||
* Date : 2020/7/8 logzhan
|
||||
*---------------------------------------------------------------------**/
|
||||
void pdr_utc2hms(double utc, double* h, double* m, double* s) {
|
||||
|
||||
long lutc = (long)(utc / 1000);
|
||||
|
||||
lutc = lutc % (3600 * 24);
|
||||
|
||||
*h = floor((double)(lutc / (3600)));
|
||||
*m = floor((lutc - (*h) * 3600) / 60);
|
||||
*s = lutc - (*h) * 3600 - (*m) * 60;
|
||||
|
||||
}
|
||||
|
|
@ -1,496 +0,0 @@
|
|||
/********************************************************************/
|
||||
/* 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; /* 重复初始化 */
|
||||
}
|
||||
|
||||
/* 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. */
|
||||
}
|
||||
}
|
||||
File diff suppressed because it is too large
Load Diff
|
|
@ -1 +0,0 @@
|
|||
.DS_store
|
||||
|
|
@ -1,95 +0,0 @@
|
|||
Statement of Purpose
|
||||
|
||||
The laws of most jurisdictions throughout the world automatically confer
|
||||
exclusive Copyright and Related Rights (defined below) upon the creator and
|
||||
subsequent owner(s) (each and all, an "owner") of an original work of
|
||||
authorship and/or a database (each, a "Work").
|
||||
|
||||
Certain owners wish to permanently relinquish those rights to a Work for the
|
||||
purpose of contributing to a commons of creative, cultural and scientific works
|
||||
("Commons") that the public can reliably and without fear of later claims of
|
||||
infringement build upon, modify, incorporate in other works, reuse and
|
||||
redistribute as freely as possible in any form whatsoever and for any purposes,
|
||||
including without limitation commercial purposes. These owners may contribute to
|
||||
the Commons to promote the ideal of a free culture and the further production of
|
||||
creative, cultural and scientific works, or to gain reputation or greater
|
||||
distribution for their Work in part through the use and efforts of others.
|
||||
|
||||
For these and/or other purposes and motivations, and without any expectation of
|
||||
additional consideration or compensation, the person associating CC0 with a Work
|
||||
(the "Affirmer"), to the extent that he or she is an owner of Copyright and
|
||||
Related Rights in the Work, voluntarily elects to apply CC0 to the Work and
|
||||
publicly distribute the Work under its terms, with knowledge of his or her
|
||||
Copyright and Related Rights in the Work and the meaning and intended legal
|
||||
effect of CC0 on those rights.
|
||||
|
||||
1. Copyright and Related Rights. A Work made available under CC0 may be
|
||||
protected by copyright and related or neighboring rights ("Copyright and Related
|
||||
Rights"). Copyright and Related Rights include, but are not limited to, the
|
||||
following:
|
||||
|
||||
the right to reproduce, adapt, distribute, perform, display, communicate, and
|
||||
translate a Work; moral rights retained by the original author(s) and/or
|
||||
performer(s); publicity and privacy rights pertaining to a person's image or
|
||||
likeness depicted in a Work; rights protecting against unfair competition in
|
||||
regards to a Work, subject to the limitations in paragraph 4(a), below; rights
|
||||
protecting the extraction, dissemination, use and reuse of data in a Work;
|
||||
database rights (such as those arising under Directive 96/9/EC of the European
|
||||
Parliament and of the Council of 11 March 1996 on the legal protection of
|
||||
databases, and under any national implementation thereof, including any amended
|
||||
or successor version of such directive); and other similar, equivalent or
|
||||
corresponding rights throughout the world based on applicable law or treaty, and
|
||||
any national implementations thereof. 2. Waiver. To the greatest extent
|
||||
permitted by, but not in contravention of, applicable law, Affirmer hereby
|
||||
overtly, fully, permanently, irrevocably and unconditionally waives, abandons,
|
||||
and surrenders all of Affirmer's Copyright and Related Rights and associated
|
||||
claims and causes of action, whether now known or unknown (including existing as
|
||||
well as future claims and causes of action), in the Work (i) in all territories
|
||||
worldwide, (ii) for the maximum duration provided by applicable law or treaty
|
||||
(including future time extensions), (iii) in any current or future medium and
|
||||
for any number of copies, and (iv) for any purpose whatsoever, including without
|
||||
limitation commercial, advertising or promotional purposes (the "Waiver").
|
||||
Affirmer makes the Waiver for the benefit of each member of the public at large
|
||||
and to the detriment of Affirmer's heirs and successors, fully intending that
|
||||
such Waiver shall not be subject to revocation, rescission, cancellation,
|
||||
termination, or any other legal or equitable action to disrupt the quiet
|
||||
enjoyment of the Work by the public as contemplated by Affirmer's express
|
||||
Statement of Purpose.
|
||||
|
||||
3. Public License Fallback. Should any part of the Waiver for any reason be
|
||||
judged legally invalid or ineffective under applicable law, then the Waiver
|
||||
shall be preserved to the maximum extent permitted taking into account
|
||||
Affirmer's express Statement of Purpose. In addition, to the extent the Waiver
|
||||
is so judged Affirmer hereby grants to each affected person a royalty-free, non
|
||||
transferable, non sublicensable, non exclusive, irrevocable and unconditional
|
||||
license to exercise Affirmer's Copyright and Related Rights in the Work (i) in
|
||||
all territories worldwide, (ii) for the maximum duration provided by applicable
|
||||
law or treaty (including future time extensions), (iii) in any current or future
|
||||
medium and for any number of copies, and (iv) for any purpose whatsoever,
|
||||
including without limitation commercial, advertising or promotional purposes
|
||||
(the "License"). The License shall be deemed effective as of the date CC0 was
|
||||
applied by Affirmer to the Work. Should any part of the License for any reason
|
||||
be judged legally invalid or ineffective under applicable law, such partial
|
||||
invalidity or ineffectiveness shall not invalidate the remainder of the License,
|
||||
and in such case Affirmer hereby affirms that he or she will not (i) exercise
|
||||
any of his or her remaining Copyright and Related Rights in the Work or (ii)
|
||||
assert any associated claims and causes of action with respect to the Work, in
|
||||
either case contrary to Affirmer's express Statement of Purpose.
|
||||
|
||||
4. Limitations and Disclaimers.
|
||||
|
||||
No trademark or patent rights held by Affirmer are waived, abandoned,
|
||||
surrendered, licensed or otherwise affected by this document. Affirmer offers
|
||||
the Work as-is and makes no representations or warranties of any kind concerning
|
||||
the Work, express, implied, statutory or otherwise, including without limitation
|
||||
warranties of title, merchantability, fitness for a particular purpose, non
|
||||
infringement, or the absence of latent or other defects, accuracy, or the
|
||||
present or absence of errors, whether or not discoverable, all to the greatest
|
||||
extent permissible under applicable law. Affirmer disclaims responsibility for
|
||||
clearing rights of other persons that may apply to the Work or any use thereof,
|
||||
including without limitation any person's Copyright and Related Rights in the
|
||||
Work. Further, Affirmer disclaims responsibility for obtaining any necessary
|
||||
consents, permissions or other rights required for any use of the Work. Affirmer
|
||||
understands and acknowledges that Creative Commons is not a party to this
|
||||
document and has no duty or obligation with respect to this CC0 or use of the
|
||||
Work.
|
||||
|
|
@ -1,244 +0,0 @@
|
|||
# NMEAParser
|
||||
|
||||
An Arduino library to parse NMEA sentences.
|
||||
|
||||
NMEA is a communication standard in the marine equipment industry: GPS, anemometers,... The NMEAParser library allows you to analyze NMEA sentences and associate handlers to each of those that need to be recognized. The library provides the means to associate a handler to each identifier and also provides functions to retrieve the arguments of the sentence in different data formats: character, integer, float, character string.
|
||||
|
||||
## Changelog
|
||||
|
||||
* ```1.1``` : Added joker in type. Remove binaries from ```extra```
|
||||
* ```1.0``` : Initial release
|
||||
|
||||
## Memory footprint
|
||||
|
||||
On an Arduino Uno, an instance of a NMEAParser requires 97 bytes with only one handler. 8 bytes per additional handler are required.
|
||||
|
||||
## Using NMEAParser
|
||||
|
||||
As usual, the library should be included at the beginning of the sketch.
|
||||
|
||||
```C++
|
||||
#include <NMEAParser.h>
|
||||
```
|
||||
|
||||
Then, you must instantiate a parser as follows:
|
||||
|
||||
```C++
|
||||
NMEAParser<4> parser;
|
||||
```
|
||||
|
||||
The ```4``` is the maximum number of handlers desired, ```parser``` is the name of the object that will allow the analysis of NMEA sentences. you can of course use the name you want.
|
||||
|
||||
In ```setup``` you configure your parser as you wish using the following functions.
|
||||
|
||||
### ```void addHandler(<type>, <handler>)```
|
||||
|
||||
where ```<type>``` is a character string and the type of sentence to recongnize, and ```<handler>``` the function to call when a sentence is recognize. ```<type>``` can be a string stored un RAM or a string stored in flash : ```F("ASTRI")```. If ```<type>``` has more than 5 characters, it is trucated.
|
||||
|
||||
For example, suppose you want to recognize the following sounder sentences which give the water depth below keel (DBK) and below surface (DBS):
|
||||
|
||||
```
|
||||
$SDDBK,...
|
||||
```
|
||||
|
||||
and
|
||||
|
||||
```
|
||||
$SDDBS,...
|
||||
```
|
||||
|
||||
You will install two handlers as follows (assuming your parsing object is named ```parser```):
|
||||
|
||||
```C++
|
||||
parser.addHandler("SDDBK", handleDepthBelowKeel);
|
||||
parser.addHanlder("SDDBS", handleDepthBelowSurface);
|
||||
```
|
||||
|
||||
```handleDepthBelowKeel``` and ```handleDepthBelowSurface``` are functions that will be called when sentences are recognized.
|
||||
|
||||
With version 1.1, ```<type>``` may include hyphens. An hyphens matches any character. For instance if you want the handler to match all sentences coming from the sounder, you would write:
|
||||
|
||||
```C++
|
||||
parser.addHandler("SD---", handleSounder);
|
||||
```
|
||||
|
||||
```handleSounder``` function would be called for any sentence with the type beginning with SD.
|
||||
|
||||
### ```void setDefaultHandler(<handler>)```
|
||||
|
||||
When a sentence is succefully parsed but none of the handler correspond to it, ```<handler>``` is called. It is a function corresponding to a ```void f(void)``` prototype. By default, no function is called.
|
||||
|
||||
### ```void setErrorHandler(<handler>)```
|
||||
|
||||
When a sentence is malformed : too long, wrong characters, etc, ```<handler>``` is called. It is a function corresponding to a ```void f(void)``` prototype. By default, no function is called.
|
||||
|
||||
### ```void setHandleCRC(<doCRC>)```
|
||||
|
||||
Specifies whether the CRC is checked or not. By default, the CRC is checked. If you do not want CRC verification, pass ```false``` to ```setHandleCRC```.
|
||||
|
||||
---
|
||||
In the handlers, you will get the arguments of the sentence, the sentence type or the error if any by using the following functions:
|
||||
|
||||
### ```bool getArg(<argnum>, <argval>)```
|
||||
|
||||
is used to get the arguments of the sentence. ```<argnum>``` is the number of the argument, starting at 0 for the argument that follows the identifier. ```<argval``` is a variable where the argument value will be stored if successful. ```getArg``` returns a boolean which is true if successfull, false if not.
|
||||
|
||||
Continuing with the example, [both sentences have the same 6 arguments](https://gpsd.gitlab.io/gpsd/NMEA.html#_dbk_depth_below_keel)
|
||||
|
||||
* Argument 0 is a float number giving the depth in feet.
|
||||
* Argument 1 is a ```f``` for feet.
|
||||
* Argument 2 is a float number giving the depth in meters.
|
||||
* Argument 3 is a ```M``` for Meters.
|
||||
* Argument 4 is a float number giving the depth in fathoms.
|
||||
* At last Argument 5 is a ```F``` for Fathoms.
|
||||
|
||||
Suppose you are interested by the depths in meters and you have two float variables to store theses data:
|
||||
|
||||
```C++
|
||||
float depthBelowKeel;
|
||||
float depthBelowSurface;
|
||||
```
|
||||
|
||||
You would implement ```handleDepthBelowKeel``` and ```handleDepthBelowSurface``` as follow:
|
||||
|
||||
```C++
|
||||
void handleDepthBelowKeel(void)
|
||||
{
|
||||
float depth;
|
||||
if (parser.getArg(2, depth))
|
||||
{
|
||||
depthBelowKeel = depth;
|
||||
}
|
||||
}
|
||||
|
||||
void handleDepthBelowSurface(void)
|
||||
{
|
||||
float depth;
|
||||
if (parser.getArg(2, depth))
|
||||
{
|
||||
depthBelowSurface = depth;
|
||||
}
|
||||
}
|
||||
```
|
||||
|
||||
### ```bool getType(<type>) / bool getType(<num>, <charType>)```
|
||||
|
||||
3 versions of ```getType``` exist. The first one puts the type of the sentence in ```<type>``` which is a ```char *```. The second one does the same but ```<type>``` is a ```String```. Return ```true``` if a type has been parsed, ```false``` otherwise. The third one puts a character of the type at position ```<num>```
|
||||
|
||||
### ```uint8_t argCount()```
|
||||
|
||||
Return the number of arguments.
|
||||
|
||||
### ```NMEA::ErrorCode error()```
|
||||
|
||||
Return the error. The returned code can be:
|
||||
|
||||
* ```NMEA::NO_ERROR```: no error;
|
||||
* ```NMEA::UNEXPECTED_CHAR```: a char which is not expected in the sentence has been encountered;
|
||||
* ```NMEA::BUFFER_FULL```: the sentence is too long to fit in the buffer;
|
||||
* ```NMEA::TYPE_TOO_LONG```: the sentence type has more than 5 characters;
|
||||
* ```NMEA::CRC_ERROR```: the CRC is wrong;
|
||||
* ```NMEA::INTERNAL_ERROR```: the internal state of the parser is wrong, should not happen by the way.
|
||||
|
||||
### Feeding characters to the parser
|
||||
|
||||
Characters are fed to the parser in ```loop```, assuming we get the characters from ```Serial```, the following way:
|
||||
|
||||
```C++
|
||||
while (Serial.available()) {
|
||||
parser << Serial.read();
|
||||
}
|
||||
```
|
||||
|
||||
This can also be done in ```serialEvent```. ```while``` could be replaced by ```if```.
|
||||
|
||||
## A complete example
|
||||
|
||||
Let's say you want to turn the Arduino's LED on and off. We define a NMEA sentence taking a single argument: 1 to turn on the LED and 0 to turn it off. The sentence can therefore be:
|
||||
|
||||
```
|
||||
$ARLED,1*43
|
||||
```
|
||||
|
||||
to turn the LED on and
|
||||
|
||||
```
|
||||
$ARLED,0*42
|
||||
```
|
||||
|
||||
to turn the LED off. We define a single handler to retrieve the argument and control the LED accordingly:
|
||||
|
||||
```C++
|
||||
void ledHandler()
|
||||
{
|
||||
Serial.print("Got ARLED with ");
|
||||
Serial.print(parser.argCount());
|
||||
Serial.println(" arguments");
|
||||
int ledState;
|
||||
if (parser.getArg(0,ledState)) {
|
||||
digitalWrite(LED_BUILTIN, ledState == 0 ? LOW : HIGH);
|
||||
}
|
||||
}
|
||||
```
|
||||
|
||||
We define 2 other handlers for anything else than ```ARLED``` and for errors
|
||||
|
||||
```C++
|
||||
void errorHandler()
|
||||
{
|
||||
Serial.print("*** Error : ");
|
||||
Serial.println(parser.error());
|
||||
}
|
||||
|
||||
void unknownCommand()
|
||||
{
|
||||
Serial.print("*** Unkown command : ");
|
||||
char buf[6];
|
||||
parser.getType(buf);
|
||||
Serial.println(buf);
|
||||
}
|
||||
```
|
||||
|
||||
In ```setup```, the handlers are installed:
|
||||
|
||||
```C++
|
||||
void setup() {
|
||||
Serial.begin(115200);
|
||||
parser.setErrorHandler(errorHandler);
|
||||
parser.addHandler("ARLED", ledHandler);
|
||||
parser.setDefaultHandler(unknownCommand);
|
||||
pinMode(LED_BUILTIN, OUTPUT);
|
||||
}
|
||||
```
|
||||
|
||||
At last in loop, we feed the parser with the chars coming from ```Serial```.
|
||||
|
||||
```C++
|
||||
void loop() {
|
||||
if (Serial.available()) {
|
||||
parser << Serial.read();
|
||||
}
|
||||
}
|
||||
```
|
||||
|
||||
---
|
||||
# Extra software
|
||||
|
||||
Additional software can be found in the ```extra``` directory.
|
||||
|
||||
## NMEA sentences generator
|
||||
|
||||
The ```gen``` subdirectory contains ```nmeagen```, a NMEA sentence generator program. This program generates well formed sentences with good or bad CRC. It can be used to test the parser. To build ```nmeagen```, run the ```build.sh``` script. ```nmeagen``` takes 1 or 2 arguments. The first argument is the number of sentences to generate. The second optional one is the number of sentences with bad CRC.
|
||||
|
||||
## Test program
|
||||
|
||||
The ```test``` subdirectory contains a test program that compile on Linux or Mac OS X. It takes sentences from the standard input, parse them and print out the type, the arguments and if an error occured.
|
||||
|
||||
---
|
||||
# Additional links
|
||||
|
||||
* [NMEA CRC calculator](https://nmeachecksum.eqth.net)
|
||||
* [NMEA 0183 on Wikipedia](https://fr.wikipedia.org/wiki/NMEA_0183)
|
||||
* [NMEA revealed](https://gpsd.gitlab.io/gpsd/NMEA.html)
|
||||
|
||||
That's all folks !
|
||||
|
|
@ -1,60 +0,0 @@
|
|||
/*
|
||||
* NMEAParser library.
|
||||
*
|
||||
* A NMEA example to switch ON or OFF the built in LED
|
||||
*
|
||||
* Sentences to send are :
|
||||
*
|
||||
* $ARLED,1*43
|
||||
*
|
||||
* to switch the LED on and
|
||||
*
|
||||
* $ARLED,0*42
|
||||
*
|
||||
* to switch the LED off
|
||||
*
|
||||
* Set the serial monitor end of line to <cr><lf>
|
||||
*/
|
||||
#include <NMEAParser.h>
|
||||
|
||||
/* A parser with only one handler */
|
||||
NMEAParser<1> parser;
|
||||
|
||||
void errorHandler()
|
||||
{
|
||||
Serial.print("*** Error : ");
|
||||
Serial.println(parser.error());
|
||||
}
|
||||
|
||||
void unknownCommand()
|
||||
{
|
||||
Serial.print("*** Unkown command : ");
|
||||
char buf[6];
|
||||
parser.getType(buf);
|
||||
Serial.println(buf);
|
||||
}
|
||||
|
||||
void ledHandler()
|
||||
{
|
||||
Serial.print("Got ARLED with ");
|
||||
Serial.print(parser.argCount());
|
||||
Serial.println(" arguments");
|
||||
int ledState;
|
||||
if (parser.getArg(0,ledState)) {
|
||||
digitalWrite(LED_BUILTIN, ledState == 0 ? LOW : HIGH);
|
||||
}
|
||||
}
|
||||
|
||||
void setup() {
|
||||
Serial.begin(115200);
|
||||
parser.setErrorHandler(errorHandler);
|
||||
parser.addHandler("ARLED", ledHandler);
|
||||
parser.setDefaultHandler(unknownCommand);
|
||||
pinMode(LED_BUILTIN, OUTPUT);
|
||||
}
|
||||
|
||||
void loop() {
|
||||
if (Serial.available()) {
|
||||
parser << Serial.read();
|
||||
}
|
||||
}
|
||||
|
|
@ -1,67 +0,0 @@
|
|||
/*
|
||||
* NMEAParser library.
|
||||
*
|
||||
* A NMEA example to switch ON or OFF the built in LED but with the use
|
||||
* of wild char in the type
|
||||
*
|
||||
* Sentences to send are :
|
||||
*
|
||||
* $ARLE1*2B
|
||||
*
|
||||
* to switch the LED on and
|
||||
*
|
||||
* $ARLE0*2A
|
||||
*
|
||||
* to switch the LED off
|
||||
*
|
||||
* Set the serial monitor end of line to <cr><lf>
|
||||
*/
|
||||
#include <NMEAParser.h>
|
||||
|
||||
/* A parser with only one handler */
|
||||
NMEAParser<1> parser;
|
||||
|
||||
void errorHandler()
|
||||
{
|
||||
Serial.print("*** Error : ");
|
||||
Serial.println(parser.error());
|
||||
}
|
||||
|
||||
void unknownCommand()
|
||||
{
|
||||
Serial.print("*** Unkown command : ");
|
||||
char buf[6];
|
||||
parser.getType(buf);
|
||||
Serial.println(buf);
|
||||
}
|
||||
|
||||
void ledHandler()
|
||||
{
|
||||
Serial.print("Got ARLEx with ");
|
||||
Serial.print(parser.argCount());
|
||||
Serial.println(" arguments");
|
||||
|
||||
char wantedLedState;
|
||||
if (parser.getType(4, wantedLedState)) { // get the 4th character ot the type
|
||||
if (wantedLedState == '0' || wantedLedState == '1') {
|
||||
digitalWrite(LED_BUILTIN, wantedLedState == '0' ? LOW : HIGH);
|
||||
}
|
||||
else {
|
||||
Serial.println("x should be 0 or 1");
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void setup() {
|
||||
Serial.begin(115200);
|
||||
parser.setErrorHandler(errorHandler);
|
||||
parser.addHandler("ARLE-", ledHandler);
|
||||
parser.setDefaultHandler(unknownCommand);
|
||||
pinMode(LED_BUILTIN, OUTPUT);
|
||||
}
|
||||
|
||||
void loop() {
|
||||
if (Serial.available()) {
|
||||
parser << Serial.read();
|
||||
}
|
||||
}
|
||||
|
|
@ -1,77 +0,0 @@
|
|||
/*
|
||||
* NMEAParser library.
|
||||
*
|
||||
* Parsing example.
|
||||
*
|
||||
* 2 handlers are added and 2 sentences are parsed
|
||||
*/
|
||||
#include <NMEAParser.h>
|
||||
|
||||
/* A parser is declared with 2 handlers at most */
|
||||
NMEAParser<2> parser;
|
||||
|
||||
const char firstSentence[] = "$DBAXK*54\r\n";
|
||||
const char secondSentence[] = "$EJCHQ,03,O,UUEIE,S,953.11,S,4.172,ASBUX,J*54\r\n";
|
||||
|
||||
void errorHandler()
|
||||
{
|
||||
Serial.print("*** Error : ");
|
||||
Serial.println(parser.error());
|
||||
}
|
||||
|
||||
void firstHandler()
|
||||
{
|
||||
Serial.print("Got DBAXK with ");
|
||||
Serial.print(parser.argCount());
|
||||
Serial.println(" arguments");
|
||||
}
|
||||
|
||||
void secondHandler()
|
||||
{
|
||||
Serial.print("Got $EJCHQ with ");
|
||||
Serial.print(parser.argCount());
|
||||
Serial.println(" arguments");
|
||||
int arg0;
|
||||
char arg1;
|
||||
char arg2[10];
|
||||
char arg3;
|
||||
float arg4;
|
||||
char arg5;
|
||||
float arg6;
|
||||
String arg7;
|
||||
char arg8;
|
||||
if (parser.getArg(0,arg0)) Serial.println(arg0);
|
||||
if (parser.getArg(1,arg1)) Serial.println(arg1);
|
||||
if (parser.getArg(2,arg2)) Serial.println(arg2);
|
||||
if (parser.getArg(3,arg3)) Serial.println(arg3);
|
||||
if (parser.getArg(4,arg4)) Serial.println(arg4);
|
||||
if (parser.getArg(5,arg5)) Serial.println(arg5);
|
||||
if (parser.getArg(6,arg6)) Serial.println(arg6);
|
||||
if (parser.getArg(7,arg7)) Serial.println(arg7);
|
||||
if (parser.getArg(8,arg8)) Serial.println(arg8);
|
||||
}
|
||||
|
||||
void setup()
|
||||
{
|
||||
Serial.begin(115200);
|
||||
|
||||
parser.setErrorHandler(errorHandler);
|
||||
parser.addHandler("DBAXK", firstHandler);
|
||||
parser.addHandler("EJCHQ", secondHandler);
|
||||
|
||||
Serial.print("Parsing : ");
|
||||
Serial.print(firstSentence);
|
||||
for (uint8_t i = 0; i < strlen(firstSentence); i++) {
|
||||
parser << firstSentence[i];
|
||||
}
|
||||
|
||||
Serial.print("Parsing : ");
|
||||
Serial.print(secondSentence);
|
||||
for (uint8_t i = 0; i < strlen(secondSentence); i++) {
|
||||
parser << secondSentence[i];
|
||||
}
|
||||
}
|
||||
|
||||
void loop()
|
||||
{
|
||||
}
|
||||
|
|
@ -1,101 +0,0 @@
|
|||
#include <stdio.h>
|
||||
#include <inttypes.h>
|
||||
#include <stdint.h>
|
||||
#include <stdbool.h>
|
||||
#include <stdlib.h>
|
||||
#include <time.h>
|
||||
|
||||
char intToHex(uint8_t v)
|
||||
{
|
||||
if (v < 10) return '0' + v;
|
||||
else if (v < 16) return 'A' + v - 10;
|
||||
else return '-';
|
||||
}
|
||||
|
||||
void generateSentence(bool goodCRC)
|
||||
{
|
||||
uint8_t crc = 0;
|
||||
putchar('$');
|
||||
/* generate the id */
|
||||
for (int i = 0; i < 5; i++) {
|
||||
char letter = 'A' + random() % 26;
|
||||
putchar(letter);
|
||||
crc ^= letter;
|
||||
}
|
||||
int numberOfArgs = random() % 10;
|
||||
for (int i = 0; i < numberOfArgs; i++) {
|
||||
putchar(',');
|
||||
crc ^= ',';
|
||||
int kind = random() % 5;
|
||||
switch (kind) {
|
||||
case 0: /* integer */
|
||||
for (int j = 0; j < random() % 5 + 1; j++) {
|
||||
char digit = '0' + random() % 10;
|
||||
putchar(digit);
|
||||
crc ^= digit;
|
||||
}
|
||||
break;
|
||||
case 1: /* char */
|
||||
if (1) {
|
||||
char letter = 'A' + random() % 26;
|
||||
putchar(letter);
|
||||
crc ^= letter;
|
||||
}
|
||||
break;
|
||||
case 2: /* string */
|
||||
for (int i = 0; i < 5; i++) {
|
||||
char letter = 'A' + random() % 26;
|
||||
putchar(letter);
|
||||
crc ^= letter;
|
||||
}
|
||||
break;
|
||||
case 3: /* float */
|
||||
for (int j = 0; j < random() % 5 + 1; j++) {
|
||||
char digit = '0' + random() % 10;
|
||||
putchar(digit);
|
||||
crc ^= digit;
|
||||
}
|
||||
putchar('.');
|
||||
crc ^= '.';
|
||||
for (int j = 0; j < random() % 5 + 1; j++) {
|
||||
char digit = '0' + random() % 10;
|
||||
putchar(digit);
|
||||
crc ^= digit;
|
||||
}
|
||||
case 4: /* nothing */
|
||||
break;
|
||||
}
|
||||
}
|
||||
if (! goodCRC) crc = ~crc;
|
||||
putchar('*');
|
||||
putchar(intToHex(crc >> 4));
|
||||
putchar(intToHex(crc & 0x0F));
|
||||
putchar('\r');
|
||||
putchar('\n');
|
||||
}
|
||||
|
||||
int main(int argc, char *argv[])
|
||||
{
|
||||
if (argc < 2 || argc > 3) {
|
||||
printf("Usage: nmeagen <n> [<c>] where <n> is the number of sentences\n"
|
||||
" and <c> the number of sentences with a wrong CRC\n");
|
||||
exit(1);
|
||||
}
|
||||
else {
|
||||
int numberOfSentences = strtol(argv[1], NULL, 0);
|
||||
int numberOfWrongCRCSentences = 0;
|
||||
if (argc == 3) {
|
||||
numberOfWrongCRCSentences = strtol(argv[2], NULL, 0);
|
||||
}
|
||||
|
||||
/* Init a the random seed with the epoch */
|
||||
srandom(time(NULL));
|
||||
|
||||
for (int i = 0; i < numberOfSentences; i++) {
|
||||
generateSentence(true);
|
||||
}
|
||||
for (int i = 0; i < numberOfWrongCRCSentences; i++) {
|
||||
generateSentence(false);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
@ -1,3 +0,0 @@
|
|||
#!/bin/bash
|
||||
|
||||
gcc -o nmeagen NMEASentencesGenerator.c
|
||||
|
|
@ -1,30 +0,0 @@
|
|||
$FRMIO,G,33,081,I,Q,Q,J,48*2E
|
||||
$APYOG,LDCXT,JSCIE,KUJAU,64,EJMVQ,F,P,AJQLD*12
|
||||
$SUHEG,GAGCE,G,Q*31
|
||||
$KIEHA,E,W,R,SXGMG,64,40,WCIEC,2253,THWNT*6E
|
||||
$WPHGT,FRYBV,5689,C*68
|
||||
$YDXHC,A,J*45
|
||||
$GUIAZ,CWIMW*2B
|
||||
$SOEOE,ZBKFY,HKTHJ,536,504,946,0,705,BIDPI,ACEOK*4B
|
||||
$TEHJW,NNHPF,4,34,DFSFJ,8638,F,B*59
|
||||
$LKGPI,XTUUO*36
|
||||
$ZCHFN,769,0202,12*4E
|
||||
$DQTMV,Y,712,0*2B
|
||||
$MQPEM,H,95,E,M,U,92,N,340,D*47
|
||||
$APUXX,RKPFP,MUQOA,VOCGB,39,6,D,V,U*78
|
||||
$MBLXP,22573,3,AVNFF*3C
|
||||
$LCBXF,V,21,AKCLF,YIASR,PHIUS,VPRJM*11
|
||||
$OFXPI,2,B,14,A*7C
|
||||
$IUKFO,D,MSETP,99,GYBDX*05
|
||||
$UDSFE,QHPNK,D,N,R*55
|
||||
$EDULQ,ISGJX,FDXLJ,O,H,M,B,ROZIU,C*4A
|
||||
$FUONE,P,Y*5E
|
||||
$VLVIU*50
|
||||
$WTBCG,14,819,ATHAO,V,MSUHV,NSCLD*76
|
||||
$WJZLK,3,Q,31*0C
|
||||
$MRFGM,NCVPP,NZCBL,F,U,C,672,LPFAS*56
|
||||
$UFOJW,V,AIUHA,BFBKN*2C
|
||||
$OHOSS,HOIBI,8695,031,B,CVSMD,154,DEUCB*79
|
||||
$MYHGK,ZKTDK,YFJBI,OLBOI,74,KPASB,XBCSE,Q,U*00
|
||||
$ZIENW*4F
|
||||
$ONAYP,N,27,A*6F
|
||||
|
|
@ -1,3 +0,0 @@
|
|||
#!/bin/bash
|
||||
|
||||
c++ -o testNMEA testNMEA.cpp
|
||||
|
|
@ -1,63 +0,0 @@
|
|||
#include "../../src/NMEAParser.h"
|
||||
#include <string.h>
|
||||
|
||||
NMEAParser<4> commandNMEA;
|
||||
int errorCount = 0;
|
||||
|
||||
void error()
|
||||
{
|
||||
printf("=================================================\n");
|
||||
int err = commandNMEA.error();
|
||||
printf("*** ERROR %d ",err);
|
||||
switch (err) {
|
||||
case NMEA::UNEXPECTED_CHAR:
|
||||
printf("(UNEXPECTED CHAR)\n");
|
||||
break;
|
||||
case NMEA::BUFFER_FULL:
|
||||
printf("(BUFFER FULL)\n");
|
||||
break;
|
||||
case NMEA::CRC_ERROR:
|
||||
printf("(CRC ERROR)\n");
|
||||
break;
|
||||
case NMEA::INTERNAL_ERROR:
|
||||
printf("(INTERNAL ERROR)\n");
|
||||
break;
|
||||
default:
|
||||
printf("(?)\n");
|
||||
break;
|
||||
}
|
||||
commandNMEA.printBuffer();
|
||||
printf("=================================================\n");
|
||||
errorCount++;
|
||||
}
|
||||
|
||||
void defaultHandler()
|
||||
{
|
||||
printf("------------\n");
|
||||
char buf[82];
|
||||
if (commandNMEA.getType(buf)) {
|
||||
printf("%s\n", buf);
|
||||
for (int i = 0; i < commandNMEA.argCount(); i++) {
|
||||
if (commandNMEA.getArg(i, buf)) {
|
||||
printf(" %s\n", buf);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
int main()
|
||||
{
|
||||
printf("Debut du test\n");
|
||||
|
||||
commandNMEA.setErrorHandler(error);
|
||||
commandNMEA.setDefaultHandler(defaultHandler);
|
||||
|
||||
int count = 0;
|
||||
int v;
|
||||
while ((v = getchar()) != EOF) {
|
||||
commandNMEA << v;
|
||||
if (v == '\n') count++;
|
||||
}
|
||||
printf("*** Processed %d NMEA sentences\n", count);
|
||||
printf("*** Got %d error(s)\n", errorCount);
|
||||
}
|
||||
|
|
@ -1,32 +0,0 @@
|
|||
#######################################
|
||||
# Syntax Coloring Map For NMEAParser
|
||||
#######################################
|
||||
|
||||
#######################################
|
||||
# Datatypes (KEYWORD1)
|
||||
#######################################
|
||||
|
||||
NMEAParser KEYWORD1
|
||||
NMEA KEYWORD1
|
||||
|
||||
#######################################
|
||||
# Methods and Functions (KEYWORD2)
|
||||
#######################################
|
||||
|
||||
addHandler KEYWORD2
|
||||
setErrorHandler KEYWORD2
|
||||
setDefaultHandler KEYWORD2
|
||||
argCount KEYWORD2
|
||||
getArg KEYWORD2
|
||||
getType KEYWORD2
|
||||
error KEYWORD2
|
||||
|
||||
#######################################
|
||||
# Constants (LITERAL1)
|
||||
#######################################
|
||||
|
||||
NO_ERROR LITERAL1
|
||||
UNEXPECTED_CHAR LITERAL1
|
||||
BUFFER_FULL LITERAL1
|
||||
CRC_ERROR LITERAL1
|
||||
INTERNAL_ERROR LITERAL1
|
||||
|
|
@ -1,9 +0,0 @@
|
|||
name=NMEAParser
|
||||
version=1.1
|
||||
author=Glinnes Hulden
|
||||
maintainer=Glinnes Hulden
|
||||
sentence=A simple Arduino library to parse NMEA sentences.
|
||||
paragraph=A simple Arduino library to parse NMEA sentences.
|
||||
category=Communication
|
||||
url=https://github.com/Glinnes/NMEAParser
|
||||
architectures=*
|
||||
|
|
@ -1,657 +0,0 @@
|
|||
/*
|
||||
* NMEA parser library for Arduino
|
||||
*
|
||||
* Simple and compact NMEA parser designed for Arduino
|
||||
*
|
||||
* Author : Glinnes Hulden
|
||||
*
|
||||
* This work is distributed under license CC0.
|
||||
* Check https://creativecommons.org/publicdomain/zero/1.0/deed.en
|
||||
*
|
||||
* No Copyright
|
||||
*
|
||||
* The person who associated a work with this deed has dedicated the work
|
||||
* to the public domain by waiving all of his or her rights to the work
|
||||
* worldwide under copyright law, including all related and neighboring rights,
|
||||
* to the extent allowed by law.
|
||||
*
|
||||
* You can copy, modify, distribute and perform the work, even for commercial
|
||||
* purposes, all without asking permission. See Other Information below.
|
||||
*
|
||||
* Other Information
|
||||
*
|
||||
* In no way are the patent or trademark rights of any person affected by CC0,
|
||||
* nor are the rights that other persons may have in the work or in how the
|
||||
* work is used, such as publicity or privacy rights.
|
||||
* Unless expressly stated otherwise, the person who associated a work with
|
||||
* this deed makes no warranties about the work, and disclaims liability for
|
||||
* all uses of the work, to the fullest extent permitted by applicable law.
|
||||
* When using or citing the work, you should not imply endorsement by the
|
||||
* author or the affirmer.
|
||||
*/
|
||||
|
||||
#ifndef __NMEAParser_h__
|
||||
#define __NMEAParser_h__
|
||||
|
||||
#ifdef __amd64__
|
||||
/* To use on my development platform */
|
||||
#include <stddef.h>
|
||||
#include <stdint.h>
|
||||
#include <ctype.h>
|
||||
#include <stdio.h>
|
||||
#include <string.h>
|
||||
#include <stdlib.h>
|
||||
#else
|
||||
#include <Arduino.h>
|
||||
#endif
|
||||
|
||||
namespace NMEA {
|
||||
/*
|
||||
* Error codes
|
||||
*/
|
||||
typedef enum {
|
||||
NO_ERROR,
|
||||
UNEXPECTED_CHAR,
|
||||
BUFFER_FULL,
|
||||
TYPE_TOO_LONG,
|
||||
CRC_ERROR,
|
||||
INTERNAL_ERROR
|
||||
} ErrorCode;
|
||||
}
|
||||
|
||||
/*
|
||||
* The library consists of a single template: NMEAParser.
|
||||
*/
|
||||
template <size_t S> class NMEAParser {
|
||||
|
||||
private:
|
||||
typedef void (*NMEAErrorHandler)(void);
|
||||
typedef void (*NMEAHandler)(void);
|
||||
typedef struct { char mToken[6]; NMEAHandler mHandler; } NMEAHandlerEntry;
|
||||
typedef enum { INIT, SENT, ARG, CRCH, CRCL, CRLFCR, CRLFLF } State;
|
||||
|
||||
public:
|
||||
/*
|
||||
* maximum sentence size is 82 including the starting '$' and the <cr><lf>
|
||||
* at the end. Since '$', the '*', the 2 characters CRC and the <cr><lf>
|
||||
* are not bufferized, 82 - 6 + 1 = 77 chars are enough.
|
||||
* is enough.
|
||||
*/
|
||||
static const uint8_t kSentenceMaxSize = 77;
|
||||
|
||||
private:
|
||||
/*
|
||||
* buffer to store the NMEA sentence excluding starting '$', the ','
|
||||
* separator, the '*', the CRC and the <cr><lf>. The tail of the buffer
|
||||
* is used to store the index of the arguments.
|
||||
*/
|
||||
char mBuffer[kSentenceMaxSize];
|
||||
|
||||
/*
|
||||
* Current index to store a char of the sentence
|
||||
*/
|
||||
uint8_t mIndex;
|
||||
|
||||
/*
|
||||
* Current index to store the index of an argument
|
||||
*/
|
||||
uint8_t mArgIndex;
|
||||
|
||||
/*
|
||||
* A handler to notify a malformed sentence
|
||||
*/
|
||||
NMEAErrorHandler mErrorHandler;
|
||||
|
||||
/*
|
||||
* A handler for well formed but unrecognized sentences
|
||||
*/
|
||||
NMEAHandler mDefaultHandler;
|
||||
|
||||
/*
|
||||
* An array of NMEA handler : pointers to functions to call when a sentence
|
||||
* is recognized
|
||||
*/
|
||||
NMEAHandlerEntry mHandlers[S];
|
||||
|
||||
/*
|
||||
* The current number of mHandlers
|
||||
*/
|
||||
uint8_t mHandlerCount;
|
||||
|
||||
/*
|
||||
* Parsing automaton variable
|
||||
*/
|
||||
State mState;
|
||||
|
||||
/*
|
||||
* mError
|
||||
*/
|
||||
NMEA::ErrorCode mError;
|
||||
|
||||
/*
|
||||
* True if CRC is handled, false otherwise. Defaults to true
|
||||
*/
|
||||
bool mHandleCRC;
|
||||
|
||||
/*
|
||||
* Variables used to computed and parse the CRC
|
||||
*/
|
||||
uint8_t mComputedCRC;
|
||||
uint8_t mGotCRC;
|
||||
|
||||
/*
|
||||
* NMEAParserStringify is used internally to temporarely replace a char
|
||||
* in the buffer by a '\0' so that libc string functions may be used.
|
||||
* Instantiating a NMEAParserStringify object in a pair of {} defines
|
||||
* a section in which the 'stringification' is done : the constructor
|
||||
* does that according to the arguments and se destructor restore the buffer.
|
||||
*/
|
||||
class NMEAParserStringify {
|
||||
uint8_t mPos;
|
||||
char mTmp;
|
||||
NMEAParser<S> *mParent;
|
||||
public:
|
||||
NMEAParserStringify(NMEAParser<S> *inParent, uint8_t inPos) :
|
||||
mPos(inPos),
|
||||
mParent(inParent)
|
||||
{
|
||||
mTmp = mParent->mBuffer[mPos];
|
||||
mParent->mBuffer[mPos] = '\0';
|
||||
}
|
||||
~NMEAParserStringify()
|
||||
{
|
||||
mParent->mBuffer[mPos] = mTmp;
|
||||
}
|
||||
};
|
||||
|
||||
/*
|
||||
* Call the error handler if defined
|
||||
*/
|
||||
void callErrorHandler(void)
|
||||
{
|
||||
if (mErrorHandler != NULL) {
|
||||
mErrorHandler();
|
||||
}
|
||||
}
|
||||
|
||||
/*
|
||||
* Reset the parser
|
||||
*/
|
||||
void reset() {
|
||||
mState = INIT;
|
||||
mIndex = 0;
|
||||
mArgIndex = kSentenceMaxSize;
|
||||
mError = NMEA::NO_ERROR;
|
||||
}
|
||||
|
||||
/*
|
||||
* Called when the parser encounter a char that should not be there
|
||||
*/
|
||||
void unexpectedChar()
|
||||
{
|
||||
mError = NMEA::UNEXPECTED_CHAR;
|
||||
callErrorHandler();
|
||||
reset();
|
||||
}
|
||||
|
||||
/*
|
||||
* Called when the buffer is full because of a malformed sentence
|
||||
*/
|
||||
void bufferFull()
|
||||
{
|
||||
mError = NMEA::BUFFER_FULL;
|
||||
callErrorHandler();
|
||||
reset();
|
||||
}
|
||||
|
||||
/*
|
||||
* Called when the type of the sentence is longer than 5 characters
|
||||
*/
|
||||
void typeTooLong()
|
||||
{
|
||||
mError = NMEA::TYPE_TOO_LONG;
|
||||
callErrorHandler();
|
||||
reset();
|
||||
}
|
||||
/*
|
||||
* Called when the CRC is wrong
|
||||
*/
|
||||
void crcError()
|
||||
{
|
||||
mError = NMEA::CRC_ERROR;
|
||||
callErrorHandler();
|
||||
reset();
|
||||
}
|
||||
|
||||
/*
|
||||
* Called when the state of the parser is not ok
|
||||
*/
|
||||
void internalError()
|
||||
{
|
||||
mError = NMEA::INTERNAL_ERROR;
|
||||
callErrorHandler();
|
||||
reset();
|
||||
}
|
||||
|
||||
/*
|
||||
* retuns true is there is at least one byte left in the buffer
|
||||
*/
|
||||
bool spaceAvail()
|
||||
{
|
||||
return (mIndex < mArgIndex);
|
||||
}
|
||||
|
||||
/*
|
||||
* convert a one hex digit char into an int. Used for the CRC
|
||||
*/
|
||||
static int8_t hexToNum(const char inChar)
|
||||
{
|
||||
if (isdigit(inChar)) return inChar - '0';
|
||||
else if (isupper(inChar) && inChar <= 'F') return inChar - 'A' + 10;
|
||||
else if (islower(inChar) && inChar <= 'f') return inChar - 'a' + 10;
|
||||
else return -1;
|
||||
}
|
||||
|
||||
static bool strnwcmp(const char *s1, const char *s2, uint8_t len)
|
||||
{
|
||||
while (len-- > 0) {
|
||||
if (*s1 != *s2 && *s1 != '-' && *s2 != '-') return false;
|
||||
s1++;
|
||||
s2++;
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
/*
|
||||
* return the slot number for a handler. -1 if not found
|
||||
*/
|
||||
int8_t getHandler(const char *inToken)
|
||||
{
|
||||
/* Look for the token */
|
||||
for (uint8_t i = 0; i < mHandlerCount; i++) {
|
||||
if (strnwcmp(mHandlers[i].mToken, inToken, 5)) {
|
||||
return i;
|
||||
}
|
||||
}
|
||||
return -1;
|
||||
}
|
||||
|
||||
/*
|
||||
* When all the sentence has been parsed, process it by calling the handler
|
||||
*/
|
||||
void processSentence()
|
||||
{
|
||||
/* Look for the token */
|
||||
uint8_t endPos = startArgPos(0);
|
||||
int8_t slot;
|
||||
{
|
||||
NMEAParserStringify stfy(this, endPos);
|
||||
slot = getHandler(mBuffer);
|
||||
}
|
||||
if (slot != -1) {
|
||||
mHandlers[slot].mHandler();
|
||||
}
|
||||
else {
|
||||
if (mDefaultHandler != NULL) {
|
||||
mDefaultHandler();
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/*
|
||||
* Return true if inArgNum corresponds to an actual argument
|
||||
*/
|
||||
bool validArgNum(uint8_t inArgNum)
|
||||
{
|
||||
return inArgNum < (kSentenceMaxSize - mArgIndex);
|
||||
}
|
||||
|
||||
/*
|
||||
* Return the start index of the inArgNum th argument
|
||||
*/
|
||||
uint8_t startArgPos(uint8_t inArgNum)
|
||||
{
|
||||
return mBuffer[kSentenceMaxSize - 1 - inArgNum];
|
||||
}
|
||||
|
||||
/*
|
||||
* Return the end index of the inArgNum th argument
|
||||
*/
|
||||
uint8_t endArgPos(uint8_t inArgNum)
|
||||
{
|
||||
return mBuffer[kSentenceMaxSize - 2 - inArgNum];
|
||||
}
|
||||
|
||||
public:
|
||||
/*
|
||||
* Constructor initialize the parser.
|
||||
*/
|
||||
NMEAParser() :
|
||||
mErrorHandler(NULL),
|
||||
mDefaultHandler(NULL),
|
||||
mHandlerCount(0),
|
||||
mError(NMEA::NO_ERROR),
|
||||
mHandleCRC(true),
|
||||
mComputedCRC(0),
|
||||
mGotCRC(0)
|
||||
{
|
||||
reset();
|
||||
}
|
||||
|
||||
/*
|
||||
* Add a sentence handler
|
||||
*/
|
||||
void addHandler(const char *inToken, NMEAHandler inHandler)
|
||||
{
|
||||
if (mHandlerCount < S) {
|
||||
if (getHandler(inToken) == -1) {
|
||||
strncpy(mHandlers[mHandlerCount].mToken, inToken, 5);
|
||||
mHandlers[mHandlerCount].mToken[5] = '\0';
|
||||
mHandlers[mHandlerCount].mHandler = inHandler;
|
||||
mHandlerCount++;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
#ifdef __AVR__
|
||||
/*
|
||||
* Add a sentence handler. Version with a token stored in flash.
|
||||
*/
|
||||
void addHandler(const __FlashStringHelper *ifsh, NMEAHandler inHandler)
|
||||
{
|
||||
char buf[6];
|
||||
PGM_P p = reinterpret_cast<PGM_P>(ifsh);
|
||||
for (uint8_t i = 0; i < 6; i++) {
|
||||
char c = pgm_read_byte(p++);
|
||||
buf[i] = c;
|
||||
if (c == '\0') break;
|
||||
}
|
||||
addHandler(buf, inHandler);
|
||||
}
|
||||
#endif
|
||||
|
||||
/*
|
||||
* Set the error handler which is called when a sentence is malformed
|
||||
*/
|
||||
void setErrorHandler(NMEAErrorHandler inHandler)
|
||||
{
|
||||
mErrorHandler = inHandler;
|
||||
}
|
||||
|
||||
/*
|
||||
* Set the default handler which is called when a sentence is well formed
|
||||
* but has no handler associated to
|
||||
*/
|
||||
void setDefaultHandler(NMEAHandler inHandler)
|
||||
{
|
||||
mDefaultHandler = inHandler;
|
||||
}
|
||||
|
||||
/*
|
||||
* Give a character to the parser
|
||||
*/
|
||||
void operator<<(char inChar)
|
||||
{
|
||||
int8_t tmp;
|
||||
|
||||
switch (mState) {
|
||||
|
||||
/* Waiting for the starting $ character */
|
||||
case INIT:
|
||||
mError = NMEA::NO_ERROR;
|
||||
if (inChar == '$') {
|
||||
mComputedCRC = 0;
|
||||
mState = SENT;
|
||||
}
|
||||
else unexpectedChar();
|
||||
break;
|
||||
|
||||
case SENT:
|
||||
if (isalnum(inChar)) {
|
||||
if (spaceAvail()) {
|
||||
if (mIndex < 5) {
|
||||
mBuffer[mIndex++] = inChar;
|
||||
mComputedCRC ^= inChar;
|
||||
}
|
||||
else {
|
||||
typeTooLong();
|
||||
}
|
||||
}
|
||||
else bufferFull();
|
||||
}
|
||||
else {
|
||||
switch(inChar) {
|
||||
case ',' :
|
||||
mComputedCRC ^= inChar;
|
||||
mBuffer[--mArgIndex] = mIndex;
|
||||
mState = ARG;
|
||||
break;
|
||||
case '*' :
|
||||
mGotCRC = 0;
|
||||
mBuffer[--mArgIndex] = mIndex;
|
||||
mState = CRCH;
|
||||
break;
|
||||
default :
|
||||
unexpectedChar();
|
||||
break;
|
||||
}
|
||||
}
|
||||
break;
|
||||
|
||||
case ARG:
|
||||
if (spaceAvail()) {
|
||||
switch(inChar) {
|
||||
case ',' :
|
||||
mComputedCRC ^= inChar;
|
||||
mBuffer[--mArgIndex] = mIndex;
|
||||
break;
|
||||
case '*' :
|
||||
mGotCRC = 0;
|
||||
mBuffer[--mArgIndex] = mIndex;
|
||||
mState = CRCH;
|
||||
break;
|
||||
default :
|
||||
mComputedCRC ^= inChar;
|
||||
mBuffer[mIndex++] = inChar;
|
||||
break;
|
||||
}
|
||||
}
|
||||
else bufferFull();
|
||||
break;
|
||||
|
||||
case CRCH:
|
||||
tmp = hexToNum(inChar);
|
||||
if (tmp != -1) {
|
||||
mGotCRC |= (uint8_t)tmp << 4;
|
||||
mState = CRCL;
|
||||
}
|
||||
else unexpectedChar();
|
||||
break;
|
||||
|
||||
case CRCL:
|
||||
tmp = hexToNum(inChar);
|
||||
if (tmp != -1) {
|
||||
mGotCRC |= (uint8_t)tmp;
|
||||
mState = CRLFCR;
|
||||
}
|
||||
else unexpectedChar();
|
||||
break;
|
||||
|
||||
case CRLFCR:
|
||||
if (inChar == '\r') {
|
||||
mState = CRLFLF;
|
||||
}
|
||||
else unexpectedChar();
|
||||
break;
|
||||
|
||||
case CRLFLF:
|
||||
if (inChar == '\n') {
|
||||
if (mHandleCRC && (mGotCRC != mComputedCRC)) {
|
||||
crcError();
|
||||
}
|
||||
else {
|
||||
processSentence();
|
||||
}
|
||||
reset();
|
||||
}
|
||||
else unexpectedChar();
|
||||
break;
|
||||
|
||||
default:
|
||||
internalError();
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
/*
|
||||
* Returns the number of arguments discovered in a well formed sentence.
|
||||
*/
|
||||
uint8_t argCount()
|
||||
{
|
||||
return kSentenceMaxSize - mArgIndex - 1;
|
||||
}
|
||||
|
||||
/*
|
||||
* Returns one of the arguments. Different versions according to data type.
|
||||
*/
|
||||
bool getArg(uint8_t num, char &arg)
|
||||
{
|
||||
if (validArgNum(num)) {
|
||||
uint8_t startPos = startArgPos(num);
|
||||
uint8_t endPos = endArgPos(num);
|
||||
arg = mBuffer[startPos];
|
||||
return (endPos - startPos) == 1;
|
||||
}
|
||||
else return false;
|
||||
}
|
||||
|
||||
bool getArg(uint8_t num, char *arg)
|
||||
{
|
||||
if (validArgNum(num)) {
|
||||
uint8_t startPos = startArgPos(num);
|
||||
uint8_t endPos = endArgPos(num);
|
||||
{
|
||||
NMEAParserStringify stfy(this, endPos);
|
||||
strcpy(arg, &mBuffer[startPos]);
|
||||
}
|
||||
return true;
|
||||
}
|
||||
else return false;
|
||||
}
|
||||
|
||||
#ifdef ARDUINO
|
||||
bool getArg(uint8_t num, String &arg)
|
||||
{
|
||||
if (validArgNum(num)) {
|
||||
uint8_t startPos = startArgPos(num);
|
||||
uint8_t endPos = endArgPos(num);
|
||||
{
|
||||
NMEAParserStringify stfy(this, endPos);
|
||||
arg = &mBuffer[startPos];
|
||||
}
|
||||
return true;
|
||||
}
|
||||
else return false;
|
||||
}
|
||||
#endif
|
||||
|
||||
bool getArg(uint8_t num, int &arg)
|
||||
{
|
||||
if (validArgNum(num)) {
|
||||
uint8_t startPos = startArgPos(num);
|
||||
uint8_t endPos = endArgPos(num);
|
||||
{
|
||||
NMEAParserStringify stfy(this, endPos);
|
||||
arg = atoi(&mBuffer[startPos]);
|
||||
}
|
||||
return true;
|
||||
}
|
||||
else return false;
|
||||
}
|
||||
|
||||
bool getArg(uint8_t num, float &arg)
|
||||
{
|
||||
if (validArgNum(num)) {
|
||||
uint8_t startPos = startArgPos(num);
|
||||
uint8_t endPos = endArgPos(num);
|
||||
{
|
||||
NMEAParserStringify stfy(this, endPos);
|
||||
arg = atof(&mBuffer[startPos]);
|
||||
}
|
||||
return true;
|
||||
}
|
||||
else return false;
|
||||
}
|
||||
/*
|
||||
* Returns the type of sentence.
|
||||
*/
|
||||
bool getType(char *arg)
|
||||
{
|
||||
if (mIndex > 0) {
|
||||
uint8_t endPos = startArgPos(0);
|
||||
{
|
||||
NMEAParserStringify stfy(this, endPos);
|
||||
strncpy(arg, mBuffer, 5);
|
||||
arg[5] = '\0';
|
||||
}
|
||||
return true;
|
||||
}
|
||||
else return false;
|
||||
}
|
||||
|
||||
#ifdef ARDUINO
|
||||
bool getType(String &arg)
|
||||
{
|
||||
if (mIndex > 0) {
|
||||
uint8_t endPos = startArgPos(0);
|
||||
{
|
||||
NMEAParserStringify stfy(this, endPos);
|
||||
arg = mBuffer;
|
||||
}
|
||||
return true;
|
||||
}
|
||||
else return false;
|
||||
}
|
||||
#endif
|
||||
|
||||
bool getType(uint8_t inIndex, char &outTypeChar)
|
||||
{
|
||||
if (mIndex > 0) {
|
||||
uint8_t endPos = startArgPos(0);
|
||||
if (inIndex < endPos) {
|
||||
outTypeChar = mBuffer[inIndex];
|
||||
return true;
|
||||
}
|
||||
else return false;
|
||||
}
|
||||
else return false;
|
||||
}
|
||||
|
||||
NMEA::ErrorCode error() {
|
||||
return mError;
|
||||
}
|
||||
|
||||
void setHandleCRC(bool inHandleCRC)
|
||||
{
|
||||
mHandleCRC = inHandleCRC;
|
||||
}
|
||||
#ifdef __amd64__
|
||||
void printBuffer()
|
||||
{
|
||||
{
|
||||
NMEAParserStringify stfy(this, startArgPos(0));
|
||||
printf("%s\n", mBuffer);
|
||||
}
|
||||
for (uint8_t i = 0; i < argCount(); i++) {
|
||||
uint8_t startPos = startArgPos(i);
|
||||
uint8_t endPos = endArgPos(i);
|
||||
{
|
||||
NMEAParserStringify stfy(this, endPos);
|
||||
printf("%s\n", &mBuffer[startPos]);
|
||||
}
|
||||
}
|
||||
}
|
||||
#endif
|
||||
};
|
||||
|
||||
#endif
|
||||
|
|
@ -1,17 +0,0 @@
|
|||
# Auto detect text files and perform LF normalization
|
||||
* text=auto
|
||||
|
||||
# Custom for Visual Studio
|
||||
*.cs diff=csharp
|
||||
|
||||
# Standard to msysgit
|
||||
*.doc diff=astextplain
|
||||
*.DOC diff=astextplain
|
||||
*.docx diff=astextplain
|
||||
*.DOCX diff=astextplain
|
||||
*.dot diff=astextplain
|
||||
*.DOT diff=astextplain
|
||||
*.pdf diff=astextplain
|
||||
*.PDF diff=astextplain
|
||||
*.rtf diff=astextplain
|
||||
*.RTF diff=astextplain
|
||||
|
|
@ -1,3 +0,0 @@
|
|||
# These are supported funding model platforms
|
||||
|
||||
custom: ['paypal.me/tilz0R']
|
||||
|
|
@ -1,385 +0,0 @@
|
|||
#Build Keil files
|
||||
*.rar
|
||||
*.o
|
||||
*.d
|
||||
*.crf
|
||||
*.htm
|
||||
*.dep
|
||||
*.map
|
||||
*.bak
|
||||
*.axf
|
||||
*.lnp
|
||||
*.lst
|
||||
*.ini
|
||||
*.scvd
|
||||
*.iex
|
||||
*.sct
|
||||
*.MajerleT
|
||||
*.tjuln
|
||||
*.tilen
|
||||
*.dbgconf
|
||||
*.uvguix
|
||||
*.uvoptx
|
||||
*.__i
|
||||
*.i
|
||||
!docs/*.txt
|
||||
RTE/
|
||||
|
||||
# IAR Settings
|
||||
**/settings/*.crun
|
||||
**/settings/*.dbgdt
|
||||
**/settings/*.cspy
|
||||
**/settings/*.cspy.*
|
||||
**/settings/*.xcl
|
||||
**/settings/*.dni
|
||||
**/settings/*.wsdt
|
||||
**/settings/*.wspos
|
||||
|
||||
# IAR Debug Exe
|
||||
**/Exe/*.sim
|
||||
|
||||
# IAR Debug Obj
|
||||
**/Obj/*.pbd
|
||||
**/Obj/*.pbd.*
|
||||
**/Obj/*.pbi
|
||||
**/Obj/*.pbi.*
|
||||
|
||||
*.TMP
|
||||
/docs_src/x_Doxyfile.doxy
|
||||
|
||||
.DS_Store
|
||||
|
||||
## Ignore Visual Studio temporary files, build results, and
|
||||
## files generated by popular Visual Studio add-ons.
|
||||
##
|
||||
## Get latest from https://github.com/github/gitignore/blob/master/VisualStudio.gitignore
|
||||
|
||||
# User-specific files
|
||||
*.suo
|
||||
*.user
|
||||
*.userosscache
|
||||
*.sln.docstates
|
||||
|
||||
# User-specific files (MonoDevelop/Xamarin Studio)
|
||||
*.userprefs
|
||||
|
||||
# Build results
|
||||
[Dd]ebug/
|
||||
[Dd]ebugPublic/
|
||||
[Rr]elease/
|
||||
[Rr]eleases/
|
||||
x64/
|
||||
x86/
|
||||
bld/
|
||||
[Bb]in/
|
||||
[Oo]bj/
|
||||
[Ll]og/
|
||||
_build/
|
||||
|
||||
# Visual Studio 2015/2017 cache/options directory
|
||||
.vs/
|
||||
# Uncomment if you have tasks that create the project's static files in wwwroot
|
||||
#wwwroot/
|
||||
|
||||
# Visual Studio 2017 auto generated files
|
||||
Generated\ Files/
|
||||
|
||||
# MSTest test Results
|
||||
[Tt]est[Rr]esult*/
|
||||
[Bb]uild[Ll]og.*
|
||||
|
||||
# NUNIT
|
||||
*.VisualState.xml
|
||||
TestResult.xml
|
||||
|
||||
# Build Results of an ATL Project
|
||||
[Dd]ebugPS/
|
||||
[Rr]eleasePS/
|
||||
dlldata.c
|
||||
|
||||
# Benchmark Results
|
||||
BenchmarkDotNet.Artifacts/
|
||||
|
||||
# .NET Core
|
||||
project.lock.json
|
||||
project.fragment.lock.json
|
||||
artifacts/
|
||||
**/Properties/launchSettings.json
|
||||
|
||||
# StyleCop
|
||||
StyleCopReport.xml
|
||||
|
||||
# Files built by Visual Studio
|
||||
*_i.c
|
||||
*_p.c
|
||||
*_i.h
|
||||
*.ilk
|
||||
*.meta
|
||||
*.obj
|
||||
*.pch
|
||||
*.pdb
|
||||
*.pgc
|
||||
*.pgd
|
||||
*.rsp
|
||||
*.sbr
|
||||
*.tlb
|
||||
*.tli
|
||||
*.tlh
|
||||
*.tmp
|
||||
*.tmp_proj
|
||||
*.log
|
||||
*.vspscc
|
||||
*.vssscc
|
||||
.builds
|
||||
*.pidb
|
||||
*.svclog
|
||||
*.scc
|
||||
*.out
|
||||
*.sim
|
||||
|
||||
# Chutzpah Test files
|
||||
_Chutzpah*
|
||||
|
||||
# Visual C++ cache files
|
||||
ipch/
|
||||
*.aps
|
||||
*.ncb
|
||||
*.opendb
|
||||
*.opensdf
|
||||
*.sdf
|
||||
*.cachefile
|
||||
*.VC.db
|
||||
*.VC.VC.opendb
|
||||
|
||||
# Visual Studio profiler
|
||||
*.psess
|
||||
*.vsp
|
||||
*.vspx
|
||||
*.sap
|
||||
|
||||
# Visual Studio Trace Files
|
||||
*.e2e
|
||||
|
||||
# TFS 2012 Local Workspace
|
||||
$tf/
|
||||
|
||||
# Guidance Automation Toolkit
|
||||
*.gpState
|
||||
|
||||
# ReSharper is a .NET coding add-in
|
||||
_ReSharper*/
|
||||
*.[Rr]e[Ss]harper
|
||||
*.DotSettings.user
|
||||
|
||||
# JustCode is a .NET coding add-in
|
||||
.JustCode
|
||||
|
||||
# TeamCity is a build add-in
|
||||
_TeamCity*
|
||||
|
||||
# DotCover is a Code Coverage Tool
|
||||
*.dotCover
|
||||
|
||||
# AxoCover is a Code Coverage Tool
|
||||
.axoCover/*
|
||||
!.axoCover/settings.json
|
||||
|
||||
# Visual Studio code coverage results
|
||||
*.coverage
|
||||
*.coveragexml
|
||||
|
||||
# NCrunch
|
||||
_NCrunch_*
|
||||
.*crunch*.local.xml
|
||||
nCrunchTemp_*
|
||||
|
||||
# MightyMoose
|
||||
*.mm.*
|
||||
AutoTest.Net/
|
||||
|
||||
# Web workbench (sass)
|
||||
.sass-cache/
|
||||
|
||||
# Installshield output folder
|
||||
[Ee]xpress/
|
||||
|
||||
# DocProject is a documentation generator add-in
|
||||
DocProject/buildhelp/
|
||||
DocProject/Help/*.HxT
|
||||
DocProject/Help/*.HxC
|
||||
DocProject/Help/*.hhc
|
||||
DocProject/Help/*.hhk
|
||||
DocProject/Help/*.hhp
|
||||
DocProject/Help/Html2
|
||||
DocProject/Help/html
|
||||
|
||||
# Click-Once directory
|
||||
publish/
|
||||
|
||||
# Publish Web Output
|
||||
*.[Pp]ublish.xml
|
||||
*.azurePubxml
|
||||
# Note: Comment the next line if you want to checkin your web deploy settings,
|
||||
# but database connection strings (with potential passwords) will be unencrypted
|
||||
*.pubxml
|
||||
*.publishproj
|
||||
|
||||
# Microsoft Azure Web App publish settings. Comment the next line if you want to
|
||||
# checkin your Azure Web App publish settings, but sensitive information contained
|
||||
# in these scripts will be unencrypted
|
||||
PublishScripts/
|
||||
|
||||
# NuGet Packages
|
||||
*.nupkg
|
||||
# The packages folder can be ignored because of Package Restore
|
||||
**/[Pp]ackages/*
|
||||
# except build/, which is used as an MSBuild target.
|
||||
!**/[Pp]ackages/build/
|
||||
# Uncomment if necessary however generally it will be regenerated when needed
|
||||
#!**/[Pp]ackages/repositories.config
|
||||
# NuGet v3's project.json files produces more ignorable files
|
||||
*.nuget.props
|
||||
*.nuget.targets
|
||||
|
||||
# Microsoft Azure Build Output
|
||||
csx/
|
||||
*.build.csdef
|
||||
|
||||
# Microsoft Azure Emulator
|
||||
ecf/
|
||||
rcf/
|
||||
|
||||
# Windows Store app package directories and files
|
||||
AppPackages/
|
||||
BundleArtifacts/
|
||||
Package.StoreAssociation.xml
|
||||
_pkginfo.txt
|
||||
*.appx
|
||||
|
||||
# Visual Studio cache files
|
||||
# files ending in .cache can be ignored
|
||||
*.[Cc]ache
|
||||
# but keep track of directories ending in .cache
|
||||
!*.[Cc]ache/
|
||||
|
||||
# Others
|
||||
ClientBin/
|
||||
~$*
|
||||
*~
|
||||
*.dbmdl
|
||||
*.dbproj.schemaview
|
||||
*.jfm
|
||||
*.pfx
|
||||
*.publishsettings
|
||||
orleans.codegen.cs
|
||||
|
||||
# Including strong name files can present a security risk
|
||||
# (https://github.com/github/gitignore/pull/2483#issue-259490424)
|
||||
#*.snk
|
||||
|
||||
# Since there are multiple workflows, uncomment next line to ignore bower_components
|
||||
# (https://github.com/github/gitignore/pull/1529#issuecomment-104372622)
|
||||
#bower_components/
|
||||
|
||||
# RIA/Silverlight projects
|
||||
Generated_Code/
|
||||
|
||||
# Backup & report files from converting an old project file
|
||||
# to a newer Visual Studio version. Backup files are not needed,
|
||||
# because we have git ;-)
|
||||
_UpgradeReport_Files/
|
||||
Backup*/
|
||||
UpgradeLog*.XML
|
||||
UpgradeLog*.htm
|
||||
|
||||
# SQL Server files
|
||||
*.mdf
|
||||
*.ldf
|
||||
*.ndf
|
||||
|
||||
# Business Intelligence projects
|
||||
*.rdl.data
|
||||
*.bim.layout
|
||||
*.bim_*.settings
|
||||
|
||||
# Microsoft Fakes
|
||||
FakesAssemblies/
|
||||
|
||||
# GhostDoc plugin setting file
|
||||
*.GhostDoc.xml
|
||||
|
||||
# Node.js Tools for Visual Studio
|
||||
.ntvs_analysis.dat
|
||||
node_modules/
|
||||
|
||||
# TypeScript v1 declaration files
|
||||
typings/
|
||||
|
||||
# Visual Studio 6 build log
|
||||
*.plg
|
||||
|
||||
# Visual Studio 6 workspace options file
|
||||
*.opt
|
||||
|
||||
# Visual Studio 6 auto-generated workspace file (contains which files were open etc.)
|
||||
*.vbw
|
||||
|
||||
# Visual Studio LightSwitch build output
|
||||
**/*.HTMLClient/GeneratedArtifacts
|
||||
**/*.DesktopClient/GeneratedArtifacts
|
||||
**/*.DesktopClient/ModelManifest.xml
|
||||
**/*.Server/GeneratedArtifacts
|
||||
**/*.Server/ModelManifest.xml
|
||||
_Pvt_Extensions
|
||||
|
||||
# Paket dependency manager
|
||||
.paket/paket.exe
|
||||
paket-files/
|
||||
|
||||
# FAKE - F# Make
|
||||
.fake/
|
||||
|
||||
# JetBrains Rider
|
||||
.idea/
|
||||
*.sln.iml
|
||||
|
||||
# CodeRush
|
||||
.cr/
|
||||
|
||||
# Python Tools for Visual Studio (PTVS)
|
||||
__pycache__/
|
||||
*.pyc
|
||||
|
||||
# Cake - Uncomment if you are using it
|
||||
# tools/**
|
||||
# !tools/packages.config
|
||||
|
||||
# Tabs Studio
|
||||
*.tss
|
||||
|
||||
# Telerik's JustMock configuration file
|
||||
*.jmconfig
|
||||
|
||||
# BizTalk build output
|
||||
*.btp.cs
|
||||
*.btm.cs
|
||||
*.odx.cs
|
||||
*.xsd.cs
|
||||
|
||||
# OpenCover UI analysis results
|
||||
OpenCover/
|
||||
|
||||
# Azure Stream Analytics local run output
|
||||
ASALocalRun/
|
||||
|
||||
# MSBuild Binary and Structured Log
|
||||
*.binlog
|
||||
|
||||
log_file.txt
|
||||
.metadata/
|
||||
.mxproject
|
||||
.settings/
|
||||
project.ioc
|
||||
mx.scratch
|
||||
*.tilen majerle
|
||||
*.exe
|
||||
|
|
@ -1,5 +0,0 @@
|
|||
" tree-specific vimrc to comply with project coding style
|
||||
" see https://github.com/MaJerle/c-code-style
|
||||
if &ft == "c" || &ft == "cpp"
|
||||
setlocal shiftwidth=4 tabstop=4 softtabstop=4 expandtab autoindent cc=80 foldmethod=indent
|
||||
endif
|
||||
|
|
@ -1,39 +0,0 @@
|
|||
################################################################################
|
||||
# Lightweight GPS (lwgps) CMake support
|
||||
################################################################################
|
||||
# The lwgps library can be configured with a lwgps_opts.h file.
|
||||
# If such a file is used, the user should set the LWGPS_CONFIG_PATH path variable where
|
||||
# the configuration file is located so that is is included properly.
|
||||
#
|
||||
# Other than that, only two steps are necessary to compile and link against LWGPS:
|
||||
# 1. Use add_subdirectory to add the lwgps folder
|
||||
# 2. Link against lwgps with target_link_libraries
|
||||
################################################################################
|
||||
cmake_minimum_required(VERSION 3.13)
|
||||
|
||||
set(LIB_LWGPS_NAME lwgps)
|
||||
|
||||
add_library(${LIB_LWGPS_NAME})
|
||||
|
||||
add_subdirectory(${LIB_LWGPS_NAME})
|
||||
add_subdirectory(examples)
|
||||
|
||||
# The project CMakeLists can set a LWGPS_CONFIG_PATH path including the lwgps_opts.h file
|
||||
# and add it.
|
||||
if(NOT LWGPS_CONFIG_PATH)
|
||||
message(STATUS "Lightweight GPS configuration path not set.")
|
||||
endif()
|
||||
|
||||
# Extract the absolute path of the provided configuration path
|
||||
if(IS_ABSOLUTE ${LWGPS_CONFIG_PATH})
|
||||
set(LWGPS_CONFIG_PATH_ABSOLUTE ${LWGPS_CONFIG_PATH})
|
||||
else()
|
||||
get_filename_component(LWGPS_CONFIG_PATH_ABSOLUTE
|
||||
${LWGPS_CONFIG_PATH} REALPATH BASE_DIR ${CMAKE_SOURCE_DIR}
|
||||
)
|
||||
endif()
|
||||
|
||||
target_include_directories(${LIB_LWGPS_NAME} PRIVATE
|
||||
${LWGPS_CONFIG_PATH_ABSOLUTE}
|
||||
)
|
||||
|
||||
|
|
@ -1,21 +0,0 @@
|
|||
MIT License
|
||||
|
||||
Copyright (c) 2020 Tilen MAJERLE
|
||||
|
||||
Permission is hereby granted, free of charge, to any person obtaining a copy
|
||||
of this software and associated documentation files (the "Software"), to deal
|
||||
in the Software without restriction, including without limitation the rights
|
||||
to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
|
||||
copies of the Software, and to permit persons to whom the Software is
|
||||
furnished to do so, subject to the following conditions:
|
||||
|
||||
The above copyright notice and this permission notice shall be included in all
|
||||
copies or substantial portions of the Software.
|
||||
|
||||
THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
|
||||
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
|
||||
FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
|
||||
AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
|
||||
LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
|
||||
OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
|
||||
SOFTWARE.
|
||||
|
|
@ -1,40 +0,0 @@
|
|||
# Lightweight GPS NMEA parser
|
||||
|
||||
Platform independent GPS NMEA parser for embedded systems.
|
||||
|
||||
<h3>Read first: <a href="http://docs.majerle.eu/projects/lwgps/">Documentation</a></h3>
|
||||
|
||||
## Features
|
||||
|
||||
* Written in ANSI C99
|
||||
* Platform independent, easy to use
|
||||
* Built-in support for 4 GPS statements
|
||||
* ``GPGGA`` or ``GNGGA``: GPS fix data
|
||||
* ``GPGSA`` or ``GNGSA``: GPS active satellites and dillusion of position
|
||||
* ``GPGSV`` or ``GNGSV``: List of satellites in view zone
|
||||
* ``GPRMC`` or ``GNRMC``: Recommended minimum specific GPS/Transit data
|
||||
* Optional ``float`` or ``double`` floating point units
|
||||
* Low-level layer is separated from application layer, thus allows you to add custom communication with GPS device
|
||||
* Works with operating systems
|
||||
* Works with different communication interfaces
|
||||
* User friendly MIT license
|
||||
|
||||
## Contribute
|
||||
|
||||
Fresh contributions are always welcome. Simple instructions to proceed::
|
||||
|
||||
1. Fork Github repository
|
||||
2. Respect [C style & coding rules](https://github.com/MaJerle/c-code-style) used by the library
|
||||
3. Create a pull request to develop branch with new features or bug fixes
|
||||
|
||||
Alternatively you may:
|
||||
|
||||
1. Report a bug
|
||||
2. Ask for a feature request
|
||||
|
||||
## Test
|
||||
|
||||
To build the code and run basic tests on your host::
|
||||
|
||||
cd examples
|
||||
make test
|
||||
|
|
@ -1,31 +0,0 @@
|
|||
|
||||
Microsoft Visual Studio Solution File, Format Version 12.00
|
||||
# Visual Studio Version 16
|
||||
VisualStudioVersion = 16.0.28803.452
|
||||
MinimumVisualStudioVersion = 10.0.40219.1
|
||||
Project("{8BC9CEB8-8B4A-11D0-8D11-00A0C91BC942}") = "lwgps_dev", "lwgps_dev.vcxproj", "{C7C465FB-17B3-4226-BBD5-E79C1B3796C6}"
|
||||
EndProject
|
||||
Global
|
||||
GlobalSection(SolutionConfigurationPlatforms) = preSolution
|
||||
Debug|x64 = Debug|x64
|
||||
Debug|x86 = Debug|x86
|
||||
Release|x64 = Release|x64
|
||||
Release|x86 = Release|x86
|
||||
EndGlobalSection
|
||||
GlobalSection(ProjectConfigurationPlatforms) = postSolution
|
||||
{C7C465FB-17B3-4226-BBD5-E79C1B3796C6}.Debug|x64.ActiveCfg = Debug|Win32
|
||||
{C7C465FB-17B3-4226-BBD5-E79C1B3796C6}.Debug|x64.Build.0 = Debug|Win32
|
||||
{C7C465FB-17B3-4226-BBD5-E79C1B3796C6}.Debug|x86.ActiveCfg = Debug|Win32
|
||||
{C7C465FB-17B3-4226-BBD5-E79C1B3796C6}.Debug|x86.Build.0 = Debug|Win32
|
||||
{C7C465FB-17B3-4226-BBD5-E79C1B3796C6}.Release|x64.ActiveCfg = Release|x64
|
||||
{C7C465FB-17B3-4226-BBD5-E79C1B3796C6}.Release|x64.Build.0 = Release|x64
|
||||
{C7C465FB-17B3-4226-BBD5-E79C1B3796C6}.Release|x86.ActiveCfg = Release|Win32
|
||||
{C7C465FB-17B3-4226-BBD5-E79C1B3796C6}.Release|x86.Build.0 = Release|Win32
|
||||
EndGlobalSection
|
||||
GlobalSection(SolutionProperties) = preSolution
|
||||
HideSolutionNode = FALSE
|
||||
EndGlobalSection
|
||||
GlobalSection(ExtensibilityGlobals) = postSolution
|
||||
SolutionGuid = {C58E98E3-45C5-48F1-A7AF-BCFEC103DD09}
|
||||
EndGlobalSection
|
||||
EndGlobal
|
||||
|
|
@ -1,149 +0,0 @@
|
|||
<?xml version="1.0" encoding="utf-8"?>
|
||||
<Project DefaultTargets="Build" ToolsVersion="15.0" xmlns="http://schemas.microsoft.com/developer/msbuild/2003">
|
||||
<ItemGroup Label="ProjectConfigurations">
|
||||
<ProjectConfiguration Include="Debug|Win32">
|
||||
<Configuration>Debug</Configuration>
|
||||
<Platform>Win32</Platform>
|
||||
</ProjectConfiguration>
|
||||
<ProjectConfiguration Include="Release|Win32">
|
||||
<Configuration>Release</Configuration>
|
||||
<Platform>Win32</Platform>
|
||||
</ProjectConfiguration>
|
||||
<ProjectConfiguration Include="Debug|x64">
|
||||
<Configuration>Debug</Configuration>
|
||||
<Platform>x64</Platform>
|
||||
</ProjectConfiguration>
|
||||
<ProjectConfiguration Include="Release|x64">
|
||||
<Configuration>Release</Configuration>
|
||||
<Platform>x64</Platform>
|
||||
</ProjectConfiguration>
|
||||
</ItemGroup>
|
||||
<PropertyGroup Label="Globals">
|
||||
<VCProjectVersion>15.0</VCProjectVersion>
|
||||
<ProjectGuid>{C7C465FB-17B3-4226-BBD5-E79C1B3796C6}</ProjectGuid>
|
||||
<Keyword>Win32Proj</Keyword>
|
||||
<RootNamespace>LwGPS</RootNamespace>
|
||||
<WindowsTargetPlatformVersion>10.0</WindowsTargetPlatformVersion>
|
||||
</PropertyGroup>
|
||||
<Import Project="$(VCTargetsPath)\Microsoft.Cpp.Default.props" />
|
||||
<PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Debug|Win32'" Label="Configuration">
|
||||
<ConfigurationType>Application</ConfigurationType>
|
||||
<UseDebugLibraries>true</UseDebugLibraries>
|
||||
<PlatformToolset>v142</PlatformToolset>
|
||||
<CharacterSet>Unicode</CharacterSet>
|
||||
</PropertyGroup>
|
||||
<PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Release|Win32'" Label="Configuration">
|
||||
<ConfigurationType>Application</ConfigurationType>
|
||||
<UseDebugLibraries>false</UseDebugLibraries>
|
||||
<PlatformToolset>v142</PlatformToolset>
|
||||
<WholeProgramOptimization>true</WholeProgramOptimization>
|
||||
<CharacterSet>Unicode</CharacterSet>
|
||||
</PropertyGroup>
|
||||
<PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Debug|x64'" Label="Configuration">
|
||||
<ConfigurationType>Application</ConfigurationType>
|
||||
<UseDebugLibraries>true</UseDebugLibraries>
|
||||
<PlatformToolset>v142</PlatformToolset>
|
||||
<CharacterSet>Unicode</CharacterSet>
|
||||
</PropertyGroup>
|
||||
<PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Release|x64'" Label="Configuration">
|
||||
<ConfigurationType>Application</ConfigurationType>
|
||||
<UseDebugLibraries>false</UseDebugLibraries>
|
||||
<PlatformToolset>v142</PlatformToolset>
|
||||
<WholeProgramOptimization>true</WholeProgramOptimization>
|
||||
<CharacterSet>Unicode</CharacterSet>
|
||||
</PropertyGroup>
|
||||
<Import Project="$(VCTargetsPath)\Microsoft.Cpp.props" />
|
||||
<ImportGroup Label="ExtensionSettings">
|
||||
</ImportGroup>
|
||||
<ImportGroup Label="Shared">
|
||||
</ImportGroup>
|
||||
<ImportGroup Label="PropertySheets" Condition="'$(Configuration)|$(Platform)'=='Debug|Win32'">
|
||||
<Import Project="$(UserRootDir)\Microsoft.Cpp.$(Platform).user.props" Condition="exists('$(UserRootDir)\Microsoft.Cpp.$(Platform).user.props')" Label="LocalAppDataPlatform" />
|
||||
</ImportGroup>
|
||||
<ImportGroup Label="PropertySheets" Condition="'$(Configuration)|$(Platform)'=='Release|Win32'">
|
||||
<Import Project="$(UserRootDir)\Microsoft.Cpp.$(Platform).user.props" Condition="exists('$(UserRootDir)\Microsoft.Cpp.$(Platform).user.props')" Label="LocalAppDataPlatform" />
|
||||
</ImportGroup>
|
||||
<ImportGroup Label="PropertySheets" Condition="'$(Configuration)|$(Platform)'=='Debug|x64'">
|
||||
<Import Project="$(UserRootDir)\Microsoft.Cpp.$(Platform).user.props" Condition="exists('$(UserRootDir)\Microsoft.Cpp.$(Platform).user.props')" Label="LocalAppDataPlatform" />
|
||||
</ImportGroup>
|
||||
<ImportGroup Label="PropertySheets" Condition="'$(Configuration)|$(Platform)'=='Release|x64'">
|
||||
<Import Project="$(UserRootDir)\Microsoft.Cpp.$(Platform).user.props" Condition="exists('$(UserRootDir)\Microsoft.Cpp.$(Platform).user.props')" Label="LocalAppDataPlatform" />
|
||||
</ImportGroup>
|
||||
<PropertyGroup Label="UserMacros" />
|
||||
<PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Debug|Win32'">
|
||||
<LinkIncremental>true</LinkIncremental>
|
||||
<IncludePath>.;..\..\lwgps\src\include;$(IncludePath)</IncludePath>
|
||||
</PropertyGroup>
|
||||
<PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Debug|x64'">
|
||||
<LinkIncremental>true</LinkIncremental>
|
||||
</PropertyGroup>
|
||||
<PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Release|Win32'">
|
||||
<LinkIncremental>false</LinkIncremental>
|
||||
</PropertyGroup>
|
||||
<PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Release|x64'">
|
||||
<LinkIncremental>false</LinkIncremental>
|
||||
</PropertyGroup>
|
||||
<ItemDefinitionGroup Condition="'$(Configuration)|$(Platform)'=='Debug|Win32'">
|
||||
<ClCompile>
|
||||
<PrecompiledHeader>NotUsing</PrecompiledHeader>
|
||||
<WarningLevel>Level3</WarningLevel>
|
||||
<Optimization>Disabled</Optimization>
|
||||
<PreprocessorDefinitions>WIN32;_DEBUG;_CONSOLE;%(PreprocessorDefinitions)</PreprocessorDefinitions>
|
||||
</ClCompile>
|
||||
<Link>
|
||||
<SubSystem>Console</SubSystem>
|
||||
</Link>
|
||||
</ItemDefinitionGroup>
|
||||
<ItemDefinitionGroup Condition="'$(Configuration)|$(Platform)'=='Debug|x64'">
|
||||
<ClCompile>
|
||||
<PrecompiledHeader>
|
||||
</PrecompiledHeader>
|
||||
<WarningLevel>Level3</WarningLevel>
|
||||
<Optimization>Disabled</Optimization>
|
||||
<PreprocessorDefinitions>_DEBUG;_CONSOLE;%(PreprocessorDefinitions)</PreprocessorDefinitions>
|
||||
</ClCompile>
|
||||
<Link>
|
||||
<SubSystem>Console</SubSystem>
|
||||
</Link>
|
||||
</ItemDefinitionGroup>
|
||||
<ItemDefinitionGroup Condition="'$(Configuration)|$(Platform)'=='Release|Win32'">
|
||||
<ClCompile>
|
||||
<WarningLevel>Level3</WarningLevel>
|
||||
<PrecompiledHeader>
|
||||
</PrecompiledHeader>
|
||||
<Optimization>MaxSpeed</Optimization>
|
||||
<FunctionLevelLinking>true</FunctionLevelLinking>
|
||||
<IntrinsicFunctions>true</IntrinsicFunctions>
|
||||
<PreprocessorDefinitions>WIN32;NDEBUG;_CONSOLE;%(PreprocessorDefinitions)</PreprocessorDefinitions>
|
||||
</ClCompile>
|
||||
<Link>
|
||||
<SubSystem>Console</SubSystem>
|
||||
<EnableCOMDATFolding>true</EnableCOMDATFolding>
|
||||
<OptimizeReferences>true</OptimizeReferences>
|
||||
</Link>
|
||||
</ItemDefinitionGroup>
|
||||
<ItemDefinitionGroup Condition="'$(Configuration)|$(Platform)'=='Release|x64'">
|
||||
<ClCompile>
|
||||
<WarningLevel>Level3</WarningLevel>
|
||||
<PrecompiledHeader>
|
||||
</PrecompiledHeader>
|
||||
<Optimization>MaxSpeed</Optimization>
|
||||
<FunctionLevelLinking>true</FunctionLevelLinking>
|
||||
<IntrinsicFunctions>true</IntrinsicFunctions>
|
||||
<PreprocessorDefinitions>NDEBUG;_CONSOLE;%(PreprocessorDefinitions)</PreprocessorDefinitions>
|
||||
</ClCompile>
|
||||
<Link>
|
||||
<SubSystem>Console</SubSystem>
|
||||
<EnableCOMDATFolding>true</EnableCOMDATFolding>
|
||||
<OptimizeReferences>true</OptimizeReferences>
|
||||
</Link>
|
||||
</ItemDefinitionGroup>
|
||||
<ItemGroup>
|
||||
<ClCompile Include="..\..\examples\test_code.c" />
|
||||
<ClCompile Include="..\..\lwgps\src\lwgps\lwgps.c" />
|
||||
<ClCompile Include="main.c" />
|
||||
</ItemGroup>
|
||||
<Import Project="$(VCTargetsPath)\Microsoft.Cpp.targets" />
|
||||
<ImportGroup Label="ExtensionTargets">
|
||||
</ImportGroup>
|
||||
</Project>
|
||||
|
|
@ -1,23 +0,0 @@
|
|||
<?xml version="1.0" encoding="utf-8"?>
|
||||
<Project ToolsVersion="4.0" xmlns="http://schemas.microsoft.com/developer/msbuild/2003">
|
||||
<ItemGroup>
|
||||
<Filter Include="Source Files">
|
||||
<UniqueIdentifier>{4FC737F1-C7A5-4376-A066-2A32D752A2FF}</UniqueIdentifier>
|
||||
<Extensions>cpp;c;cc;cxx;def;odl;idl;hpj;bat;asm;asmx</Extensions>
|
||||
</Filter>
|
||||
<Filter Include="Source Files\LwGPS">
|
||||
<UniqueIdentifier>{d2afbed4-545f-40e7-bf68-d835c5948bcc}</UniqueIdentifier>
|
||||
</Filter>
|
||||
</ItemGroup>
|
||||
<ItemGroup>
|
||||
<ClCompile Include="..\..\lwgps\src\lwgps\lwgps.c">
|
||||
<Filter>Source Files\LwGPS</Filter>
|
||||
</ClCompile>
|
||||
<ClCompile Include="main.c">
|
||||
<Filter>Source Files</Filter>
|
||||
</ClCompile>
|
||||
<ClCompile Include="..\..\examples\test_code.c">
|
||||
<Filter>Source Files</Filter>
|
||||
</ClCompile>
|
||||
</ItemGroup>
|
||||
</Project>
|
||||
|
|
@ -1,44 +0,0 @@
|
|||
/**
|
||||
* \file lwgps_opts_template.h
|
||||
* \brief LwGPS configuration file
|
||||
*/
|
||||
|
||||
/*
|
||||
* Copyright (c) 2020 Tilen MAJERLE
|
||||
*
|
||||
* Permission is hereby granted, free of charge, to any person
|
||||
* obtaining a copy of this software and associated documentation
|
||||
* files (the "Software"), to deal in the Software without restriction,
|
||||
* including without limitation the rights to use, copy, modify, merge,
|
||||
* publish, distribute, sublicense, and/or sell copies of the Software,
|
||||
* and to permit persons to whom the Software is furnished to do so,
|
||||
* subject to the following conditions:
|
||||
*
|
||||
* The above copyright notice and this permission notice shall be
|
||||
* included in all copies or substantial portions of the Software.
|
||||
*
|
||||
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
|
||||
* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES
|
||||
* OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE
|
||||
* AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT
|
||||
* HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
|
||||
* WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
|
||||
* FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
|
||||
* OTHER DEALINGS IN THE SOFTWARE.
|
||||
*
|
||||
* This file is part of LwGPS - Lightweight GPS NMEA parser library.
|
||||
*
|
||||
* Author: Tilen MAJERLE <tilen@majerle.eu>
|
||||
* Version: $2.1.0$
|
||||
*/
|
||||
#ifndef LWGPS_HDR_OPTS_H
|
||||
#define LWGPS_HDR_OPTS_H
|
||||
|
||||
/* Rename this file to "lwgps_opts.h" for your application */
|
||||
|
||||
/*
|
||||
* Open "include/lwgps/lwgps_opt.h" and
|
||||
* copy & replace here settings you want to change values
|
||||
*/
|
||||
|
||||
#endif /* LWGPS_HDR_OPTS_H */
|
||||
|
|
@ -1,32 +0,0 @@
|
|||
/*
|
||||
* This example uses direct processing function,
|
||||
* to process dummy NMEA data from GPS receiver
|
||||
*/
|
||||
#include <string.h>
|
||||
#include <stdio.h>
|
||||
#include <math.h>
|
||||
#include "lwgps/lwgps.h"
|
||||
|
||||
/* External function */
|
||||
extern void run_tests();
|
||||
|
||||
int
|
||||
main() {
|
||||
lwgps_float_t distance, bearing;
|
||||
run_tests();
|
||||
|
||||
/* Calculate distance and bearing */
|
||||
lwgps_distance_bearing(40.6, -73.7, 48.3, 11.7, &distance, &bearing);
|
||||
|
||||
printf("Distance: %lf meters\r\n", (double)distance);
|
||||
printf("Bearing: %lf degrees\r\n", (double)bearing);
|
||||
|
||||
lwgps_distance_bearing(48.3, 11.7, 40.6, -73.7, &distance, &bearing);
|
||||
printf("Distance: %lf meters\r\n", (double)distance);
|
||||
printf("Bearing: %lf degrees\r\n", (double)bearing);
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
/* JFK: 40.642569, -73.783790 */
|
||||
/* Munich: 48.353962, 11.775114 */
|
||||
|
|
@ -1,20 +0,0 @@
|
|||
# Minimal makefile for Sphinx documentation
|
||||
#
|
||||
|
||||
# You can set these variables from the command line, and also
|
||||
# from the environment for the first two.
|
||||
SPHINXOPTS ?=
|
||||
SPHINXBUILD ?= sphinx-build
|
||||
SOURCEDIR = .
|
||||
BUILDDIR = _build
|
||||
|
||||
# Put it first so that "make" without argument is like "make help".
|
||||
help:
|
||||
@$(SPHINXBUILD) -M help "$(SOURCEDIR)" "$(BUILDDIR)" $(SPHINXOPTS) $(O)
|
||||
|
||||
.PHONY: help Makefile
|
||||
|
||||
# Catch-all target: route all unknown targets to Sphinx using the new
|
||||
# "make mode" option. $(O) is meant as a shortcut for $(SPHINXOPTS).
|
||||
%: Makefile
|
||||
@$(SPHINXBUILD) -M $@ "$(SOURCEDIR)" "$(BUILDDIR)" $(SPHINXOPTS) $(O)
|
||||
|
|
@ -1,6 +0,0 @@
|
|||
.. _api_lwgps:
|
||||
|
||||
LwGPS
|
||||
=====
|
||||
|
||||
.. doxygengroup:: LWGPS
|
||||
|
|
@ -1,12 +0,0 @@
|
|||
.. _api_lwgps_opt:
|
||||
|
||||
Configuration
|
||||
=============
|
||||
|
||||
This is the default configuration of the middleware.
|
||||
When any of the settings shall be modified, it shall be done in dedicated application config ``lwgps_opts.h`` file.
|
||||
|
||||
.. note::
|
||||
Check :ref:`getting_started` to create configuration file.
|
||||
|
||||
.. doxygengroup:: LWGPS_OPT
|
||||
|
|
@ -1,13 +0,0 @@
|
|||
.. _api_reference:
|
||||
|
||||
API reference
|
||||
=============
|
||||
|
||||
List of all the modules:
|
||||
|
||||
.. toctree::
|
||||
:maxdepth: 2
|
||||
:glob:
|
||||
|
||||
*
|
||||
|
||||
|
|
@ -1,129 +0,0 @@
|
|||
# Configuration file for the Sphinx documentation builder.
|
||||
#
|
||||
# This file only contains a selection of the most common options. For a full
|
||||
# list see the documentation:
|
||||
# https://www.sphinx-doc.org/en/master/usage/configuration.html
|
||||
|
||||
# -- Path setup --------------------------------------------------------------
|
||||
|
||||
# If extensions (or modules to document with autodoc) are in another directory,
|
||||
# add these directories to sys.path here. If the directory is relative to the
|
||||
# documentation root, use os.path.abspath to make it absolute, like shown here.
|
||||
#
|
||||
# import os
|
||||
# import sys
|
||||
# sys.path.insert(0, os.path.abspath('.'))
|
||||
from sphinx.builders.html import StandaloneHTMLBuilder
|
||||
import subprocess, os
|
||||
|
||||
# Run doxygen first
|
||||
# read_the_docs_build = os.environ.get('READTHEDOCS', None) == 'True'
|
||||
# if read_the_docs_build:
|
||||
subprocess.call('doxygen doxyfile.doxy', shell=True)
|
||||
# -- Project information -----------------------------------------------------
|
||||
|
||||
project = 'LwGPS'
|
||||
copyright = '2020, Tilen MAJERLE'
|
||||
author = 'Tilen MAJERLE'
|
||||
|
||||
# The full version, including alpha/beta/rc tags
|
||||
version = 'v2.1.0'
|
||||
|
||||
# Try to get branch at which this is running
|
||||
# and try to determine which version to display in sphinx
|
||||
git_branch = ''
|
||||
res = os.popen('git branch').read().strip()
|
||||
for line in res.split("\n"):
|
||||
if line[0] == '*':
|
||||
git_branch = line[1:].strip()
|
||||
|
||||
# Decision for display version
|
||||
try:
|
||||
if git_branch.index('develop') >= 0:
|
||||
version = "latest-develop"
|
||||
except Exception:
|
||||
print("Exception for index check")
|
||||
|
||||
# For debugging purpose
|
||||
print("GIT BRANCH: " + git_branch)
|
||||
print("VERSION: " + version)
|
||||
|
||||
# -- General configuration ---------------------------------------------------
|
||||
|
||||
# Add any Sphinx extension module names here, as strings. They can be
|
||||
# extensions coming with Sphinx (named 'sphinx.ext.*') or your custom
|
||||
# ones.
|
||||
extensions = [
|
||||
'sphinx.ext.autodoc',
|
||||
'sphinx.ext.intersphinx',
|
||||
'sphinx.ext.autosectionlabel',
|
||||
'sphinx.ext.todo',
|
||||
'sphinx.ext.coverage',
|
||||
'sphinx.ext.mathjax',
|
||||
'sphinx.ext.ifconfig',
|
||||
'sphinx.ext.viewcode',
|
||||
'sphinx_sitemap',
|
||||
|
||||
'breathe',
|
||||
]
|
||||
|
||||
# Add any paths that contain templates here, relative to this directory.
|
||||
templates_path = ['templates']
|
||||
|
||||
# List of patterns, relative to source directory, that match files and
|
||||
# directories to ignore when looking for source files.
|
||||
# This pattern also affects html_static_path and html_extra_path.
|
||||
exclude_patterns = ['_build', 'Thumbs.db', '.DS_Store']
|
||||
|
||||
highlight_language = 'c'
|
||||
|
||||
# -- Options for HTML output -------------------------------------------------
|
||||
|
||||
# The theme to use for HTML and HTML Help pages. See the documentation for
|
||||
# a list of builtin themes.
|
||||
#
|
||||
html_theme = 'sphinx_rtd_theme'
|
||||
html_theme_options = {
|
||||
'canonical_url': '',
|
||||
'analytics_id': '', # Provided by Google in your dashboard
|
||||
'display_version': True,
|
||||
'prev_next_buttons_location': 'bottom',
|
||||
'style_external_links': False,
|
||||
|
||||
'logo_only': False,
|
||||
|
||||
# Toc options
|
||||
'collapse_navigation': True,
|
||||
'sticky_navigation': True,
|
||||
'navigation_depth': 4,
|
||||
'includehidden': True,
|
||||
'titles_only': False
|
||||
}
|
||||
html_logo = 'static/images/logo.svg'
|
||||
github_url = 'https://github.com/MaJerle/lwgps'
|
||||
html_baseurl = 'https://docs.majerle.eu/projects/lwgps/'
|
||||
|
||||
# Add any paths that contain custom static files (such as style sheets) here,
|
||||
# relative to this directory. They are copied after the builtin static files,
|
||||
# so a file named "default.css" will overwrite the builtin "default.css".
|
||||
html_static_path = ['static']
|
||||
html_css_files = [
|
||||
'css/common.css',
|
||||
'css/custom.css',
|
||||
]
|
||||
html_js_files = [
|
||||
'https://cdnjs.cloudflare.com/ajax/libs/font-awesome/5.15.1/css/all.min.css'
|
||||
]
|
||||
|
||||
master_doc = 'index'
|
||||
|
||||
#
|
||||
# Breathe configuration
|
||||
#
|
||||
#
|
||||
#
|
||||
breathe_projects = {
|
||||
"lwgps": "_build/xml/"
|
||||
}
|
||||
breathe_default_project = "lwgps"
|
||||
breathe_default_members = ('members', 'undoc-members')
|
||||
File diff suppressed because it is too large
Load Diff
|
|
@ -1,47 +0,0 @@
|
|||
.. _examples:
|
||||
|
||||
Examples and demos
|
||||
==================
|
||||
|
||||
There are several basic examples provided with the library.
|
||||
|
||||
Parse block of data
|
||||
^^^^^^^^^^^^^^^^^^^
|
||||
|
||||
In this example, block of data is prepared as big string array and sent to processing function in single shot.
|
||||
Application can then check if GPS signal has been detected as valid and use other data accordingly.
|
||||
|
||||
.. literalinclude:: ../../examples/example.c
|
||||
:language: c
|
||||
:linenos:
|
||||
:caption: Minimum example code
|
||||
|
||||
Parse received data from interrupt/DMA
|
||||
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
|
||||
|
||||
Second example is a typical use case with interrupts on embedded systems.
|
||||
For each received character, application uses ``ringbuff`` as intermediate buffer.
|
||||
Data are later processed outside interrupt context.
|
||||
|
||||
.. note::
|
||||
For the sake of this example, application *implements* interrupts as function call in *while loop*.
|
||||
|
||||
.. literalinclude:: ../../examples/example_buff.c
|
||||
:language: c
|
||||
:linenos:
|
||||
:caption: Example of buffer
|
||||
|
||||
Distance and bearing
|
||||
^^^^^^^^^^^^^^^^^^^^
|
||||
|
||||
Library provides calculation of distance and bearing between ``2`` coordinates on earth.
|
||||
This is useful if used with autonomnous devices to understand in which direction
|
||||
device has to move to reach end point while knowing start coordinate.
|
||||
|
||||
.. literalinclude:: ../../examples/example_dist_bear.c
|
||||
:language: c
|
||||
:linenos:
|
||||
:caption: Distance and bearing calculation
|
||||
|
||||
.. toctree::
|
||||
:maxdepth: 2
|
||||
|
|
@ -1,100 +0,0 @@
|
|||
.. _getting_started:
|
||||
|
||||
Getting started
|
||||
===============
|
||||
|
||||
.. _download_library:
|
||||
|
||||
Download library
|
||||
^^^^^^^^^^^^^^^^
|
||||
|
||||
Library is primarly hosted on `Github <https://github.com/MaJerle/lwgps>`_.
|
||||
|
||||
* Download latest release from `releases area <https://github.com/MaJerle/lwgps/releases>`_ on Github
|
||||
* Clone `develop` branch for latest development
|
||||
|
||||
Download from releases
|
||||
**********************
|
||||
|
||||
All releases are available on Github `releases area <https://github.com/MaJerle/lwgps/releases>`_.
|
||||
|
||||
Clone from Github
|
||||
*****************
|
||||
|
||||
First-time clone
|
||||
""""""""""""""""
|
||||
|
||||
* Download and install ``git`` if not already
|
||||
* Open console and navigate to path in the system to clone repository to. Use command ``cd your_path``
|
||||
* Clone repository with one of available ``3`` options
|
||||
|
||||
* Run ``git clone --recurse-submodules https://github.com/MaJerle/lwgps`` command to clone entire repository, including submodules
|
||||
* Run ``git clone --recurse-submodules --branch develop https://github.com/MaJerle/lwgps`` to clone `development` branch, including submodules
|
||||
* Run ``git clone --recurse-submodules --branch master https://github.com/MaJerle/lwgps`` to clone `latest stable` branch, including submodules
|
||||
|
||||
* Navigate to ``examples`` directory and run favourite example
|
||||
|
||||
Update cloned to latest version
|
||||
"""""""""""""""""""""""""""""""
|
||||
|
||||
* Open console and navigate to path in the system where your resources repository is. Use command ``cd your_path``
|
||||
* Run ``git pull origin master --recurse-submodules`` command to pull latest changes and to fetch latest changes from submodules
|
||||
* Run ``git submodule foreach git pull origin master`` to update & merge all submodules
|
||||
|
||||
.. note::
|
||||
This is preferred option to use when you want to evaluate library and run prepared examples.
|
||||
Repository consists of multiple submodules which can be automatically downloaded when cloning and pulling changes from root repository.
|
||||
|
||||
|
||||
Add library to project - Generic
|
||||
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
|
||||
|
||||
At this point it is assumed that you have successfully download library, either cloned it or from releases page.
|
||||
|
||||
* Copy ``lwgps`` folder to your project
|
||||
* Add ``lwgps/src/include`` folder to `include path` of your toolchain
|
||||
* Add source files from ``lwgps/src/`` folder to toolchain build
|
||||
* Copy ``lwgps/src/include/lwgps/lwgps_opts_template.h`` to project folder and rename it to ``lwgps_opts.h``
|
||||
* Build the project
|
||||
|
||||
Add library to project - CMake
|
||||
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
|
||||
|
||||
Including the library with CMake is very easy.
|
||||
|
||||
* Add the ``lwgps`` folder with ``add_subdirectory``
|
||||
* Copy ``lwgps/src/include/lwgps/lwgps_opts_template.h`` to the project folder and rename it
|
||||
to ``lwgps_opts.h``
|
||||
* Set the ``LWGPS_CONFIG_PATH`` path variable containing the ``lwgps_opts.h`` file
|
||||
in the project ``CMakeLists.txt``
|
||||
* Link your project against the ``lwgps`` library with ``target_link_libraries``
|
||||
* The include directory should be set automatically by CMake
|
||||
|
||||
Configuration file
|
||||
^^^^^^^^^^^^^^^^^^
|
||||
|
||||
Library comes with template config file, which can be modified according to needs.
|
||||
This file shall be named ``lwgps_opts.h`` and its default template looks like the one below.
|
||||
|
||||
.. note::
|
||||
Default configuration template file location: ``lwgps/src/include/lwgps/lwgps_opts_template.h``.
|
||||
File must be renamed to ``lwgps_opts.h`` first and then copied to the project directory (or simply renamed in-place) where compiler
|
||||
include paths have access to it by using ``#include "lwgps_opts.h"``.
|
||||
|
||||
.. tip::
|
||||
Check :ref:`api_lwgps_opt` section for possible configuration settings
|
||||
|
||||
.. literalinclude:: ../../lwgps/src/include/lwgps/lwgps_opts_template.h
|
||||
:language: c
|
||||
:linenos:
|
||||
:caption: Template options file
|
||||
|
||||
Minimal example code
|
||||
^^^^^^^^^^^^^^^^^^^^
|
||||
|
||||
Run below example to test and verify library
|
||||
|
||||
.. literalinclude:: ../../examples/example.c
|
||||
:language: c
|
||||
:linenos:
|
||||
:caption: Test verification code
|
||||
|
|
@ -1,71 +0,0 @@
|
|||
LwGPS |version| documentation
|
||||
=============================
|
||||
|
||||
Welcome to the documentation for version |version|.
|
||||
|
||||
LwGPS is lightweight, platform independent library to parse NMEA statements from GPS receivers. It is highly optimized for embedded systems.
|
||||
|
||||
.. image:: static/images/logo.svg
|
||||
:align: center
|
||||
|
||||
.. rst-class:: center
|
||||
.. rst-class:: index_links
|
||||
|
||||
:ref:`download_library` :ref:`getting_started` `Open Github <https://github.com/MaJerle/lwgps>`_ `Donate <https://paypal.me/tilz0R>`_
|
||||
|
||||
Features
|
||||
^^^^^^^^
|
||||
|
||||
* Written in ANSI C99
|
||||
* Platform independent, easy to use
|
||||
* Built-in support for 4 GPS statements
|
||||
|
||||
* ``GPGGA`` or ``GNGGA``: GPS fix data
|
||||
* ``GPGSA`` or ``GNGSA``: GPS active satellites and dillusion of position
|
||||
* ``GPGSV`` or ``GNGSV``: List of satellites in view zone
|
||||
* ``GPRMC`` or ``GNRMC``: Recommended minimum specific GPS/Transit data
|
||||
|
||||
* Optional ``float`` or ``double`` floating point units
|
||||
* Low-level layer is separated from application layer, thus allows you to add custom communication with GPS device
|
||||
* Works with operating systems
|
||||
* Works with different communication interfaces
|
||||
* User friendly MIT license
|
||||
|
||||
Requirements
|
||||
^^^^^^^^^^^^
|
||||
|
||||
* C compiler
|
||||
* Driver for receiving data from GPS receiver
|
||||
* Few *kB* of non-volatile memory
|
||||
|
||||
Contribute
|
||||
^^^^^^^^^^
|
||||
|
||||
Fresh contributions are always welcome. Simple instructions to proceed:
|
||||
|
||||
#. Fork Github repository
|
||||
#. Respect `C style & coding rules <https://github.com/MaJerle/c-code-style>`_ used by the library
|
||||
#. Create a pull request to ``develop`` branch with new features or bug fixes
|
||||
|
||||
Alternatively you may:
|
||||
|
||||
#. Report a bug
|
||||
#. Ask for a feature request
|
||||
|
||||
License
|
||||
^^^^^^^
|
||||
|
||||
.. literalinclude:: ../LICENSE
|
||||
|
||||
Table of contents
|
||||
^^^^^^^^^^^^^^^^^
|
||||
|
||||
.. toctree::
|
||||
:maxdepth: 2
|
||||
|
||||
self
|
||||
get-started/index
|
||||
user-manual/index
|
||||
api-reference/index
|
||||
examples/index
|
||||
|
||||
|
|
@ -1,35 +0,0 @@
|
|||
@ECHO OFF
|
||||
|
||||
pushd %~dp0
|
||||
|
||||
REM Command file for Sphinx documentation
|
||||
|
||||
if "%SPHINXBUILD%" == "" (
|
||||
set SPHINXBUILD=sphinx-build
|
||||
)
|
||||
set SOURCEDIR=.
|
||||
set BUILDDIR=_build
|
||||
|
||||
if "%1" == "" goto help
|
||||
|
||||
%SPHINXBUILD% >NUL 2>NUL
|
||||
if errorlevel 9009 (
|
||||
echo.
|
||||
echo.The 'sphinx-build' command was not found. Make sure you have Sphinx
|
||||
echo.installed, then set the SPHINXBUILD environment variable to point
|
||||
echo.to the full path of the 'sphinx-build' executable. Alternatively you
|
||||
echo.may add the Sphinx directory to PATH.
|
||||
echo.
|
||||
echo.If you don't have Sphinx installed, grab it from
|
||||
echo.http://sphinx-doc.org/
|
||||
exit /b 1
|
||||
)
|
||||
|
||||
%SPHINXBUILD% -M %1 %SOURCEDIR% %BUILDDIR% %SPHINXOPTS% %O%
|
||||
goto end
|
||||
|
||||
:help
|
||||
%SPHINXBUILD% -M help %SOURCEDIR% %BUILDDIR% %SPHINXOPTS% %O%
|
||||
|
||||
:end
|
||||
popd
|
||||
|
|
@ -1,8 +0,0 @@
|
|||
breathe>=4.9.1
|
||||
colorama
|
||||
docutils>=0.14
|
||||
sphinx>=2.0.1
|
||||
sphinx_rtd_theme
|
||||
sphinx-tabs
|
||||
sphinxcontrib-svg2pdfconverter
|
||||
sphinx-sitemap
|
||||
|
|
@ -1,64 +0,0 @@
|
|||
/* Center aligned text */
|
||||
.center {
|
||||
text-align: center;
|
||||
}
|
||||
|
||||
/* Paragraph with main links on index page */
|
||||
.index-links {
|
||||
text-align: center;
|
||||
margin-top: 10px;
|
||||
}
|
||||
.index-links a {
|
||||
display: inline-block;
|
||||
border: 1px solid #0E4263;
|
||||
padding: 5px 20px;
|
||||
margin: 2px 5px;
|
||||
background: #2980B9;
|
||||
border-radius: 4px;
|
||||
color: #FFFFFF;
|
||||
}
|
||||
.index-links a:hover, .index-links a:active {
|
||||
background: #0E4263;
|
||||
}
|
||||
|
||||
/* Table header p w/0 margin */
|
||||
.index-links a table thead th {
|
||||
vertical-align: middle;
|
||||
}
|
||||
|
||||
table thead th p {
|
||||
margin: 0;
|
||||
}
|
||||
|
||||
.table-nowrap td {
|
||||
white-space: normal !important;
|
||||
}
|
||||
|
||||
/* Breathe output changes */
|
||||
.breathe-sectiondef.container {
|
||||
background: #f9f9f9;
|
||||
padding: 10px;
|
||||
margin-bottom: 10px;
|
||||
border: 1px solid #efefef;
|
||||
}
|
||||
.breathe-sectiondef.container .breathe-sectiondef-title {
|
||||
background: #2980b9;
|
||||
color: #FFFFFF;
|
||||
padding: 4px;
|
||||
margin: -10px -10px 0 -10px;
|
||||
}
|
||||
.breathe-sectiondef.container .function,
|
||||
.breathe-sectiondef.container .member,
|
||||
.breathe-sectiondef.container .class,
|
||||
.breathe-sectiondef.container .type {
|
||||
border-bottom: 1px solid #efefef;
|
||||
}
|
||||
.breathe-sectiondef.container .function:last-child,
|
||||
.breathe-sectiondef.container .member:last-child,
|
||||
.breathe-sectiondef.container .class:last-child,
|
||||
.breathe-sectiondef.container .type:last-child {
|
||||
border-bottom: none;
|
||||
margin-bottom: 0;
|
||||
}
|
||||
|
||||
/*# sourceMappingURL=common.css.map */
|
||||
File diff suppressed because one or more lines are too long
|
Before Width: | Height: | Size: 30 KiB |
|
|
@ -1 +0,0 @@
|
|||
<mxfile modified="2019-03-17T18:59:38.494Z" host="www.draw.io" agent="Mozilla/5.0 (Windows NT 10.0; Win64; x64) AppleWebKit/537.36 (KHTML, like Gecko) draw.io/7.8.7 Chrome/58.0.3029.110 Electron/1.7.5 Safari/537.36" version="10.4.9" etag="K-7s2_uXvQkXh2xx6gpu" type="device"><diagram id="7c61e978-b8e5-555b-ca9a-a1ec85429089" name="Page-1">7Ztfb5swFMU/TR4n2RgIPC5d1z5s0rR06rMDJlgBHDlOk+zTz4BpSM2kaupyq6vmITXXf8C/wwPn0MzYTX2803xbfle5qGYByY8z9mUWBDQkzP5pK6e+ktB5X1hrmbtB58JS/hauSFx1L3OxuxholKqM3F4WM9U0IjMXNa61OlwOK1R1edYtXwuvsMx45VcfZW5KV6Vxeu64F3JdulMngdvfimebtVb7xp1vFrCi+/TdNR/WchvdlTxXh1GJ3c7YjVbK9K36eCOqlu2ArZ/39S+9z9etRWNeMyHoJzzxau+2vnxwV2ZOA41uP6KdQWZscSilEcstz9reg9Xf1kpTV/aI2mbFV6JaPGO4UZXStqtRjR2/KFRjnNw0cMfDEMuKMWI/tu7vw23tSWgjjqOS29edULUw+mSHuF4W9TPcLRikDvnhLChzpXIkZehq3N1C6+eFzxhtw5Gcpso8qiuChGoMRzX0qVIkVFM4qrFPNUBClQZwWOc+VoYFawSHNfGxhliwJnBYUx9rhASrvV3BsA6Ps2OuMRauISBX6nOdY+E6B+Q6YQd+IOHKCCBX3xDgsVkJgeM6YQmwGK2QAnKNfK5YrFYYAnJFbLbCOSBXxG4rIoBcEdutiAFyRey3ohiO65CqY/RbUQrIFbHfigNAroj9VhwBcvX91uJkhK2oxn79+vzTui9SiafutWU3gKykGfrpp0epxXlEXNlrW6y0ba3b1oOsZbNuz7frkbTblvVWacMtr8BeH7l/tF+Gb0Q7JOPdcqpoxxl/QSSSv3jjxtiU4v9Lct8Kfkh+ffd/Xc19m7osrSKtDLIW7xN6IatqVM8jkeShre+MVhsx6kmsh4njN5LpFS/Dhzx3rFLyFir5phdN+JVAPjxMmF4s4VcC+fAwYXqxhF8JYFg79ZIRS/iVAoa1w7oYw68UMKxlE6YXS/iVAoa1zDe9aMIvSgDTWjbxb4dY0i9KAONaNvGaEUv8RQlgXssmDByW/IsSwMCW+Z7rIw25vh28ahrCfD/4TXUq9WFIK8pOtS0iGyO03m87wa0IJJdFIbqNt5rtNoO0K9HrLI4i2xurx7tU7mWkwkVSZFORSpwlYlW8Ub79CitFwwmt/yFTsYfnn4R0faPf3bDbPw==</diagram></mxfile>
|
||||
File diff suppressed because one or more lines are too long
|
Before Width: | Height: | Size: 8.8 KiB |
|
|
@ -1 +0,0 @@
|
|||
<mxfile modified="2019-03-17T18:28:51.770Z" host="www.draw.io" agent="Mozilla/5.0 (Windows NT 10.0; Win64; x64) AppleWebKit/537.36 (KHTML, like Gecko) draw.io/7.8.7 Chrome/58.0.3029.110 Electron/1.7.5 Safari/537.36" etag="Lw4IH-C7q6o9UbXhWxg1" version="10.4.9" type="device"><diagram id="oLfy0yLwNNLmw2rXhwVB" name="Page-1">7VpNc9owEP01HMnYkmXsYyBpy6Fpm5CU9tIRtjCaGIsRopD++sq2DJZlkuCYkHaSQ0a7+rL2vd2VNunAwXzzkePF7DMLSdwBVrjpwIsOAD2/J3+niodcgaCfKyJOw1xl7xQ39A9R45RyRUOy1MYJxmJBF7oyYElCAqHpMOdsrQ+bsljfdIEjYihuAhyb2u80FDOltV1/1/GJ0GimtvaAOu8cF4OtXLGc4ZCtSyp42YEDzpjIW/PNgMSp6Qqz5PM+7OndfhgniXjOhC+Mrn6Ovv2KRugD/0a7v4bTUVet8hvHK3Xgz4Nbqfhy3QFuLNftT7hsRWnr9qbfSfdw8Xwhxay3uxWzIbL/9vx6pA4sHgorrmdUkJsFDlJ5LXkix87EPJaSLZt4ucixm9INkZ/bn9I4HrCY8Ww6nHoBCQKpXwrO7kmpZ+IhB1nZzlVrFEcjXJBNSaWs85GwORH8QQ4pei2FlGIq8JS8LgFfjJmVMN8qsSJbtF17h4dsKEgOgMcz4DFMS5LwPOW5lBKWEN2wZEPFuNT+IdvWmfStJibOewovAI8ZfclWPCBP846EmveZ0JRMj2osX+g4ibGgv3WfrUND7fCVUfnFO+Q9pCEPLaQvkZ9HzSr7WGWhrbPvW0hgHhFhLJSRY3vs5nyxTX82CMPZKglTL7tIQX3CM0/iiNKhdEe0TUcEj7ChdTcsNmvuh4n8jHHmfKgQfzQ28QGOmPPtGYx5I55YdSDgVzBt6onAfV1PBH5LoVsxpgjejQiTzr7YqOmZ8KCEA6hUYchzgvxpqWT7Zz3kucByPd9FyAY6ISxwBm0LOo68sqXDvHaIZnvgVYkGzdh0NxiYtzeDfTIYC51y+wm0j3I4plEidYEkC5Gd/TTIU3l7PlcdcxqG6Ya1eUbPRC0kDtvX87jdMxNHHePAsRIHfEY+fjQKHDU5vNWY7zWM+Q56YqFjuyJ4R/tgtCFq6a796mjDljK8/jjrvT/Oqo8zHWcHtnQlNJh3bMIggzB29/vw+tJM1heXd8NBjd4g2KGFlRhPSNzHwX2UJd+CSYqcp3ntuRU/tsyk7dbQy20haUcArybjcLwZXlm3V5zeB5NJ97+FycCkBrm9MEHnjcHkvsNUA5NTzYqnhqn3wixpJrbHiimZsHvqZlLx1m2Kh/HwbVJnyYP/yS5dfktXbGhXK6VHu3TVsumlBfF/gU2nIgnstUWS6kLIP/NKP/6rUualhbhTUmZXIUnXVoYinEqbEJ4mKppEnXL55O2y68nHeeNX/knZVfMXl9FYyl+HVwbNzIpb+/WzF10ebKRHd1DU00oUcZBJkTbqZ/XWNSsq1/+NdaFzNOtKcfePBTnVd/+cAS//Ag==</diagram></mxfile>
|
||||
File diff suppressed because one or more lines are too long
|
Before Width: | Height: | Size: 11 KiB |
|
|
@ -1 +0,0 @@
|
|||
<mxfile modified="2019-03-17T18:24:31.030Z" host="www.draw.io" agent="Mozilla/5.0 (Windows NT 10.0; Win64; x64) AppleWebKit/537.36 (KHTML, like Gecko) draw.io/7.8.7 Chrome/58.0.3029.110 Electron/1.7.5 Safari/537.36" version="10.4.9" etag="GRhl1qzylYBum7KQYHDi" type="device"><diagram id="85174833-9250-f023-2320-fd733ea284ef" name="Page-1">7Vxdd6I6FP01PraLBMLHY9V2pg8zd1Y7HWceKUTNGgQH01Hvr78BgkKigsiHt8WXkgMEOHvvk5ND6EAdLTafQns5/xK42BtAxd0M1PEAQqApKvsTWbaJxQRGYpiFxOUH7Q3P5F+cGBE3vhEXr3LH0SDwKFnmjU7g+9ihOZsdhsE6f9g08PIXXdozLBmeHduTrRPi0jl/CKTs7Z8xmc3TKwOF73m1nd+zMHjz+fUGUJ3Gv2T3wk774sev5rYbrDMm9X6gjsIgoMnWYjPCXuTa1GvJeQ9H9u7uO8Q+LXOCNRk++KOvD5//jP3J2Jj88TXnBqT9/LW9N5w+SHy7dJu6CPvuXeRp1vIDnxmHc7rwWAuwTbwh9CfbVm4Rb/2KWmx7SjxvFHhBGPeiTk0HOw6zr2gY/MaZPa8m0pDCzx5v+OlxY8sbyTkpQMx3w+QusSuhu/cH2HmZkRcHC0zDLTtkvYc5RXmeQTi1hdizKfmb797mbJvtuttd4VtA2IWhwoUBLU5wLgtgqvkuVsFb6GB+Vha3oo4QyndE7XCGqdQR28g89t4U0+IMiiBdYsiX0Qsz/PM0gLrHvDZ8DdnWLNp6eWbAMAfq9mLJmvHem10zPoTtf7l7+i6xbD0nFD8vbSdqr1msyfPMXi0T+U/JBruV6MWfA4cUb84lDT9BwAIanCwZTu0CRJZUO+MhAuUQOxseoyYBg6x8byGq5OLqOmU3HAuikIldCRqYSh57S9BhaUGr6HRHTQvaLGZMPK5FMoujb4E0O1Ei1MT4KksRnmBD7ULUlXc+kpZRqNnpkIu0WwOZOlR009IRAjDPEAPeqkBRNc2E8WFmRf0icUCGrepXl1O2H6ORPBZL7GNio3nKHSfQMcrZHpn5zOYwbmC2cxiJmLB0+o7vWBDXjS54MGjkw0oTUUCTo4B+gHGwsSigNTIcG/1wLA7HRh55TYS0rJxVkO9I1dodjnUkMQbcTB6f7mVFj+9/PI4O2CWGnZtLe/Yr9oa7CW1KJc7OTsZ3VciSwIFU+5Cy9caULc+DepyY7sC14VQivT0ZgeWgyVwRbjOpWdT8lcboqLFPteJWmmtVBaRMeE6iUmFc6So8q7owyRHz7tLh2RKmXYbQUdPh2foAdOqKJZpWE0ukjqB1a2Z+VqucMeQM/ftP1v72+FXijpyUX1mKDZAgQBNJAT71fisptgEl7z69G+/u0p6uvGvJ3j0v3rU5S+ms0qAJqInl3dK1BKEjKS1qOFJZao/22WhXr/yibiu/1qXFiQ+BtjDxrK5tsc7ftrblwkKPdnEArkvbYkdNoy2XJ06h7Xj2akWcDwi4iJNZ19AtdtQ04HW9+E1fESWV5vTd0+G3R1dWggaKcU1cqj5UFJGyYS4Bpc8MSsAt1rQq54FGt3kgUPrUoALcVYtTkrrFjhqH+7zcoIf7MnUX8aZxuEtkBjQktj87WufKYO+SEDuUBFFtjLkgQqJGPtT69l8F5dYAAeU4OS4qngFw6XLaD6G0oqlS5cCqtqy0dIVRD/cpuIWFk9WzZHEFZssFFQD6aun5cFcfR8XF1m1nyaCfFJWA26hrDixOilpXdz8pqqDuymO3qO6W66UA9AXTcoiLQFUumIrUabvIBfqKKXNCt0v0rbpGiyJWNk6mEssM38ck27y6SXaJJXn/gw+YBL92/gETSK/WJ0DnDIe1JUBtFy/gpbNZGbz3t0JVQqm2FaptV4VhP5stsWy96GOgql8V1Qc3a+7/aURy+P4fc6j3/wE=</diagram></mxfile>
|
||||
|
|
@ -1 +0,0 @@
|
|||
<mxfile host="Electron" modified="2020-12-05T00:17:49.072Z" agent="Mozilla/5.0 (Windows NT 10.0; Win64; x64) AppleWebKit/537.36 (KHTML, like Gecko) draw.io/12.3.2 Chrome/78.0.3904.113 Electron/7.1.2 Safari/537.36" etag="J3RPSOEyywhlMJWRwdgH" version="12.3.2" type="device" pages="1"><diagram id="Wsjaadh77UIxB9X1bxos" name="Page-1">jZJNb8MgDIZ/TY6VkpB067Vd10nrqkk59IyCG1AhRJSMdL9+ZDH5UDVpJ/BjY+B9HZGd6g6GNvxDM5BRGrMuIi9RmibxU+aXntwHssnzAVRGMCyaQCG+IZxE2goGt0Wh1Vpa0SxhqesaSrtg1BjtlmUXLZe3NrSCB1CUVD7Ss2CWD/Q5jyf+BqLi4eYkxoyioRjBjVOm3QyRfUR2Rms77FS3A9mLF3QZzr3+kR0fZqC2/zlwWrEMNsodT6rdZpv3izwnK+zyRWWLHz66w2eBL7b3IIPRbc2g75REZOu4sFA0tOyzzhvvGbdKYvpmjb6Ocq09uejaorekj0ctYh88fiS8CoyFbobwYwfQCqy5+xLMkgynCqcsDbGbPEvWaASf+RV8pDgm1dh6UtJvUMwQTqb95majT/Y/</diagram></mxfile>
|
||||
|
|
@ -1,3 +0,0 @@
|
|||
<?xml version="1.0" encoding="UTF-8"?>
|
||||
<!DOCTYPE svg PUBLIC "-//W3C//DTD SVG 1.1//EN" "http://www.w3.org/Graphics/SVG/1.1/DTD/svg11.dtd">
|
||||
<svg xmlns="http://www.w3.org/2000/svg" xmlns:xlink="http://www.w3.org/1999/xlink" version="1.1" width="166px" height="56px" viewBox="-0.5 -0.5 166 56" content="<mxfile host="Electron" modified="2020-12-05T00:17:54.452Z" agent="Mozilla/5.0 (Windows NT 10.0; Win64; x64) AppleWebKit/537.36 (KHTML, like Gecko) draw.io/12.3.2 Chrome/78.0.3904.113 Electron/7.1.2 Safari/537.36" etag="QrTFHCUJlsVW3_ulGR16" version="12.3.2" type="device" pages="1"><diagram id="Wsjaadh77UIxB9X1bxos" name="Page-1">jZJNb8MgDIZ/TY6VkpB067Vd10nrqkk59IyCG1AhRJSMdL9+ZDH5UDVpJ/BjY+B9HZGd6g6GNvxDM5BRGrMuIi9RmibxU+aXntwHssnzAVRGMCyaQCG+IZxE2goGt0Wh1Vpa0SxhqesaSrtg1BjtlmUXLZe3NrSCB1CUVD7Ss2CWD/Q5jyf+BqLi4eYkxoyioRjBjVOm3QyRfUR2Rms77FS3A9mLF3QZzr3+kR0fZqC2/zlwWrEMNsodT6rdZpv3izwnK+zyRWWLHz66w2eBL7b3IIPRbc2g75REZOu4sFA0tOyzzhvvGbdKYvpmjb6Ocq09uejaorekj0ctYh88fiS8CoyFbobwYwfQCqy5+xLMkgynCqcsDbGbPEvWaASf+RV8pDgm1dh6UtJvUMwQTqb95majT/Y/</diagram></mxfile>"><defs/><g><rect x="3" y="3" width="160" height="50" rx="7.5" ry="7.5" fill="#ffffff" stroke="#000000" stroke-width="6" pointer-events="all"/><g transform="translate(21.5,6.5)"><switch><foreignObject style="overflow:visible;" pointer-events="all" width="122" height="41" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility"><div xmlns="http://www.w3.org/1999/xhtml" style="display: inline-block; font-size: 36px; font-family: Helvetica; color: rgb(0, 0, 0); line-height: 1.2; vertical-align: top; width: 123px; white-space: nowrap; overflow-wrap: normal; text-align: center;"><div xmlns="http://www.w3.org/1999/xhtml" style="display:inline-block;text-align:inherit;text-decoration:inherit;white-space:normal;">LwGPS</div></div></foreignObject><text x="61" y="39" fill="#000000" text-anchor="middle" font-size="36px" font-family="Helvetica">LwGPS</text></switch></g></g></svg>
|
||||
|
Before Width: | Height: | Size: 2.0 KiB |
Binary file not shown.
|
Before Width: | Height: | Size: 3.3 KiB |
Binary file not shown.
|
Before Width: | Height: | Size: 17 KiB |
|
|
@ -1,15 +0,0 @@
|
|||
.. _float_double:
|
||||
|
||||
Float/double precision
|
||||
======================
|
||||
|
||||
With configuration of ``GSM_CFG_DOUBLE``, it is possible to enable ``double`` floating point precision.
|
||||
All floating point variables are then configured in *double precision*.
|
||||
|
||||
When configuration is set to ``0``, floating point variables are configured in *single precision* format.
|
||||
|
||||
.. note::
|
||||
Single precision uses less memory in application. As a drawback, application may be a subject of data loss at latter digits.
|
||||
|
||||
.. toctree::
|
||||
:maxdepth: 2
|
||||
|
|
@ -1,38 +0,0 @@
|
|||
.. _how_it_works:
|
||||
|
||||
How it works
|
||||
============
|
||||
|
||||
LwGPS parses raw data formatted as NMEA 0183 statements from GPS receivers. It supports up to ``4`` different statements:
|
||||
|
||||
* ``GPGGA`` or ``GNGGA``: GPS fix data
|
||||
* ``GPGSA`` or ``GNGSA``: GPS active satellites and dillusion of position
|
||||
* ``GPGSV`` or ``GNGSV``: List of satellites in view zone
|
||||
* ``GPRMC`` or ``GNRMC``: Recommended minimum specific GPS/Transit data
|
||||
|
||||
.. tip::
|
||||
By changing different configuration options, it is possible to disable some statements.
|
||||
Check :ref:`api_lwgps_opt` for more information.
|
||||
|
||||
Application must assure to properly receive data from GPS receiver.
|
||||
Usually GPS receivers communicate with host embedded system with UART protocol and output directly formatted NMEA 0183 statements.
|
||||
|
||||
.. note::
|
||||
Application must take care of properly receive data from GPS.
|
||||
|
||||
Application must use :cpp:func:`lwgps_process` function for data processing. Function will:
|
||||
|
||||
* Detect statement type, such as *GPGGA* or *GPGSV*
|
||||
* Parse all the terms of specific statement
|
||||
* Check valid CRC after each statement
|
||||
|
||||
Programmer's model is as following:
|
||||
|
||||
* Application receives data from GPS receiver
|
||||
* Application sends data to :cpp:func:`lwgps_process` function
|
||||
* Application uses processed data to display altitude, latitude, longitude, and other parameters
|
||||
|
||||
Check :ref:`examples` for typical example
|
||||
|
||||
.. toctree::
|
||||
:maxdepth: 2
|
||||
|
|
@ -1,12 +0,0 @@
|
|||
.. _um:
|
||||
|
||||
User manual
|
||||
===========
|
||||
|
||||
.. toctree::
|
||||
:maxdepth: 2
|
||||
|
||||
how-it-works
|
||||
float-double
|
||||
thread-safety
|
||||
tests
|
||||
|
|
@ -1,14 +0,0 @@
|
|||
.. _tests:
|
||||
|
||||
Tests during development
|
||||
========================
|
||||
|
||||
During the development, test check is performed to validate raw NMEA input data vs expected result.
|
||||
|
||||
.. literalinclude:: ../../examples/test_code.c
|
||||
:language: c
|
||||
:linenos:
|
||||
:caption: Test code for development
|
||||
|
||||
.. toctree::
|
||||
:maxdepth: 2
|
||||
|
|
@ -1,16 +0,0 @@
|
|||
.. _thread_safety:
|
||||
|
||||
Thread safety
|
||||
=============
|
||||
|
||||
Library tends to be as simple as possible.
|
||||
No specific features have been implemented for thread safety.
|
||||
|
||||
When library is using multi-thread environment and if multi threads tend to access to shared resources,
|
||||
user must resolve it with care, using mutual exclusion.
|
||||
|
||||
.. tip::
|
||||
When single thread is dedicated for GPS processing, no special mutual exclusion is necessary.
|
||||
|
||||
.. toctree::
|
||||
:maxdepth: 2
|
||||
|
|
@ -1,40 +0,0 @@
|
|||
# gps-nmea-parser examples and tests Makefile
|
||||
# (c) 2020 Sirio, Balmelli Analog & Digital
|
||||
|
||||
TARGETS := \
|
||||
example.exe \
|
||||
example_stat.exe \
|
||||
test_code.exe \
|
||||
test_time.exe
|
||||
|
||||
.PHONY: all clean test
|
||||
all: $(TARGETS)
|
||||
|
||||
clean:
|
||||
@rm -fv $(TARGETS)
|
||||
|
||||
test: $(TARGETS)
|
||||
@for tgt in $(TARGETS); do \
|
||||
echo "\n--- $$tgt ---"; \
|
||||
./$$tgt; \
|
||||
done
|
||||
|
||||
CFLAGS += -Wall \
|
||||
-DDEBUG=1 \
|
||||
-I../lwgps/src/include \
|
||||
-I./
|
||||
|
||||
example.exe: example.c
|
||||
|
||||
example_stat.exe: CFLAGS += -DLWGPS_CFG_STATUS=1
|
||||
example_stat.exe: example_stat.c
|
||||
|
||||
test_code.exe: ../lwgps/src/lwgps/lwgps.c test_code.c ../dev/VisualStudio/main.c
|
||||
|
||||
test_time.exe: CFLAGS += \
|
||||
-DLWGPS_CFG_STATEMENT_PUBX=1 \
|
||||
-DLWGPS_CFG_STATEMENT_PUBX_TIME=1
|
||||
test_time.exe: ../lwgps/src/lwgps/lwgps.c test_time.c ../dev/VisualStudio/main.c
|
||||
|
||||
$(TARGETS) : ../lwgps/src/lwgps/lwgps.c
|
||||
$(CC) -o $@ $(CFLAGS) $^
|
||||
|
|
@ -1,47 +0,0 @@
|
|||
/**
|
||||
* This example uses direct processing function
|
||||
* to process dummy NMEA data from GPS receiver
|
||||
*/
|
||||
#include <string.h>
|
||||
#include <stdio.h>
|
||||
#include "lwgps/lwgps.h"
|
||||
|
||||
/* GPS handle */
|
||||
lwgps_t hgps;
|
||||
|
||||
/**
|
||||
* \brief Dummy data from GPS receiver
|
||||
*/
|
||||
const char
|
||||
gps_rx_data[] = ""
|
||||
"$GPRMC,183729,A,3907.356,N,12102.482,W,000.0,360.0,080301,015.5,E*6F\r\n"
|
||||
"$GPRMB,A,,,,,,,,,,,,V*71\r\n"
|
||||
"$GPGGA,183730,3907.356,N,12102.482,W,1,05,1.6,646.4,M,-24.1,M,,*75\r\n"
|
||||
"$GPGSA,A,3,02,,,07,,09,24,26,,,,,1.6,1.6,1.0*3D\r\n"
|
||||
"$GPGSV,2,1,08,02,43,088,38,04,42,145,00,05,11,291,00,07,60,043,35*71\r\n"
|
||||
"$GPGSV,2,2,08,08,02,145,00,09,46,303,47,24,16,178,32,26,18,231,43*77\r\n"
|
||||
"$PGRME,22.0,M,52.9,M,51.0,M*14\r\n"
|
||||
"$GPGLL,3907.360,N,12102.481,W,183730,A*33\r\n"
|
||||
"$PGRMZ,2062,f,3*2D\r\n"
|
||||
"$PGRMM,WGS84*06\r\n"
|
||||
"$GPBOD,,T,,M,,*47\r\n"
|
||||
"$GPRTE,1,1,c,0*07\r\n"
|
||||
"$GPRMC,183731,A,3907.482,N,12102.436,W,000.0,360.0,080301,015.5,E*67\r\n"
|
||||
"$GPRMB,A,,,,,,,,,,,,V*71\r\n";
|
||||
|
||||
int
|
||||
main() {
|
||||
/* Init GPS */
|
||||
lwgps_init(&hgps);
|
||||
|
||||
/* Process all input data */
|
||||
lwgps_process(&hgps, gps_rx_data, strlen(gps_rx_data));
|
||||
|
||||
/* Print messages */
|
||||
printf("Valid status: %d\r\n", hgps.is_valid);
|
||||
printf("Latitude: %f degrees\r\n", hgps.latitude);
|
||||
printf("Longitude: %f degrees\r\n", hgps.longitude);
|
||||
printf("Altitude: %f meters\r\n", hgps.altitude);
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
|
@ -1,81 +0,0 @@
|
|||
#include "lwgps/lwgps.h"
|
||||
#include "lwrb/lwrb.h"
|
||||
#include <string.h>
|
||||
|
||||
/* GPS handle */
|
||||
lwgps_t hgps;
|
||||
|
||||
/* GPS buffer */
|
||||
lwrb_t hgps_buff;
|
||||
uint8_t hgps_buff_data[12];
|
||||
|
||||
/**
|
||||
* \brief Dummy data from GPS receiver
|
||||
* \note This data are used to fake UART receive event on microcontroller
|
||||
*/
|
||||
const char
|
||||
gps_rx_data[] = ""
|
||||
"$GPRMC,183729,A,3907.356,N,12102.482,W,000.0,360.0,080301,015.5,E*6F\r\n"
|
||||
"$GPRMB,A,,,,,,,,,,,,V*71\r\n"
|
||||
"$GPGGA,183730,3907.356,N,12102.482,W,1,05,1.6,646.4,M,-24.1,M,,*75\r\n"
|
||||
"$GPGSA,A,3,02,,,07,,09,24,26,,,,,1.6,1.6,1.0*3D\r\n"
|
||||
"$GPGSV,2,1,08,02,43,088,38,04,42,145,00,05,11,291,00,07,60,043,35*71\r\n"
|
||||
"$GPGSV,2,2,08,08,02,145,00,09,46,303,47,24,16,178,32,26,18,231,43*77\r\n"
|
||||
"$PGRME,22.0,M,52.9,M,51.0,M*14\r\n"
|
||||
"$GPGLL,3907.360,N,12102.481,W,183730,A*33\r\n"
|
||||
"$PGRMZ,2062,f,3*2D\r\n"
|
||||
"$PGRMM,WGS84*06\r\n"
|
||||
"$GPBOD,,T,,M,,*47\r\n"
|
||||
"$GPRTE,1,1,c,0*07\r\n"
|
||||
"$GPRMC,183731,A,3907.482,N,12102.436,W,000.0,360.0,080301,015.5,E*67\r\n"
|
||||
"$GPRMB,A,,,,,,,,,,,,V*71\r\n";
|
||||
static size_t write_ptr;
|
||||
static void uart_irqhandler(void);
|
||||
|
||||
int
|
||||
main() {
|
||||
uint8_t rx;
|
||||
|
||||
/* Init GPS */
|
||||
lwgps_init(&hgps);
|
||||
|
||||
/* Create buffer for received data */
|
||||
lwrb_init(&hgps_buff, hgps_buff_data, sizeof(hgps_buff_data));
|
||||
|
||||
while (1) {
|
||||
/* Add new character to buffer */
|
||||
/* Fake UART interrupt handler on host microcontroller */
|
||||
uart_irqhandler();
|
||||
|
||||
/* Process all input data */
|
||||
/* Read from buffer byte-by-byte and call processing function */
|
||||
if (lwrb_get_full(&hgps_buff)) { /* Check if anything in buffer now */
|
||||
while (lwrb_read(&hgps_buff, &rx, 1) == 1) {
|
||||
lwgps_process(&hgps, &rx, 1); /* Process byte-by-byte */
|
||||
}
|
||||
} else {
|
||||
/* Print all data after successful processing */
|
||||
printf("Latitude: %f degrees\r\n", hgps.latitude);
|
||||
printf("Longitude: %f degrees\r\n", hgps.longitude);
|
||||
printf("Altitude: %f meters\r\n", hgps.altitude);
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
/**
|
||||
* \brief Interrupt handler routing for UART received character
|
||||
* \note This is not real MCU, it is software method, called from main
|
||||
*/
|
||||
static void
|
||||
uart_irqhandler(void) {
|
||||
/* Make interrupt handler as fast as possible */
|
||||
/* Only write to received buffer and process later */
|
||||
if (write_ptr < strlen(gps_rx_data)) {
|
||||
/* Write to buffer only */
|
||||
lwrb_write(&hgps_buff, &gps_rx_data[write_ptr], 1);
|
||||
++write_ptr;
|
||||
}
|
||||
}
|
||||
|
|
@ -1,24 +0,0 @@
|
|||
#include "lwgps/lwgps.h"
|
||||
|
||||
/* Distance and bearing results */
|
||||
lwgps_float_t dist, bear;
|
||||
|
||||
/* New York coordinates */
|
||||
lwgps_float_t lat1 = 40.685721;
|
||||
lwgps_float_t lon1 = -73.820465;
|
||||
|
||||
/* Munich coordinates */
|
||||
lwgps_float_t lat2 = 48.150906;
|
||||
lwgps_float_t lon2 = 11.554176;
|
||||
|
||||
/* Go from New York to Munich */
|
||||
/* Calculate distance and bearing related to north */
|
||||
lwgps_distance_bearing(lat1, lon1, lat2, lon2, &dist, &bear);
|
||||
printf("Distance: %f meters\r\n", (float)dist);
|
||||
printf("Initial bearing: %f degrees\r\n", (float)bear);
|
||||
|
||||
/* Go from Munich to New York */
|
||||
/* Calculate distance and bearing related to north */
|
||||
lwgps_distance_bearing(lat2, lon2, lat1, lon1, &dist, &bear);
|
||||
printf("Distance: %f meters\r\n", (float)dist);
|
||||
printf("Initial bearing: %f degrees\r\n", (float)bear);
|
||||
|
|
@ -1,77 +0,0 @@
|
|||
/**
|
||||
* This example tests the callback functionality of lwgps_process()
|
||||
* when the LWGPS_CFG_STATUS flag is set.
|
||||
*/
|
||||
#include <string.h>
|
||||
#include <stdio.h>
|
||||
#include "lwgps/lwgps.h"
|
||||
|
||||
#if !LWGPS_CFG_STATUS
|
||||
#error "this test must be compiled with -DLWGPS_CFG_STATUS=1"
|
||||
#endif /* !LWGPS_CFG_STATUS */
|
||||
|
||||
/* GPS handle */
|
||||
lwgps_t hgps;
|
||||
|
||||
/**
|
||||
* \brief Dummy data from GPS receiver
|
||||
*/
|
||||
const char
|
||||
gps_rx_data[] = ""
|
||||
"$GPRMC,183729,A,3907.356,N,12102.482,W,000.0,360.0,080301,015.5,E*6F\r\n"
|
||||
"$GPRMB,A,,,,,,,,,,,,V*71\r\n"
|
||||
"$GPGGA,183730,3907.356,N,12102.482,W,1,05,1.6,646.4,M,-24.1,M,,*75\r\n"
|
||||
"$GPGSA,A,3,02,,,07,,09,24,26,,,,,1.6,1.6,1.0*3D\r\n"
|
||||
"$GPGSV,2,1,08,02,43,088,38,04,42,145,00,05,11,291,00,07,60,043,35*71\r\n"
|
||||
"$GPGSV,2,2,08,08,02,145,00,09,46,303,47,24,16,178,32,26,18,231,43*77\r\n"
|
||||
"$PGRME,22.0,M,52.9,M,51.0,M*14\r\n"
|
||||
"$GPGLL,3907.360,N,12102.481,W,183730,A*33\r\n"
|
||||
"$PGRMZ,2062,f,3*2D\r\n"
|
||||
"$PGRMM,WGS84*06\r\n"
|
||||
"$GPBOD,,T,,M,,*47\r\n"
|
||||
"$GPRTE,1,1,c,0*07\r\n"
|
||||
"$GPRMC,183731,A,3907.482,N,12102.436,W,000.0,360.0,080301,015.5,E*67\r\n"
|
||||
"$GPRMB,A,,,,,,,,,,,,V*71\r\n";
|
||||
|
||||
const lwgps_statement_t expected[] = {
|
||||
STAT_RMC,
|
||||
STAT_UNKNOWN,
|
||||
STAT_GGA,
|
||||
STAT_GSA,
|
||||
STAT_GSV,
|
||||
STAT_GSV,
|
||||
STAT_UNKNOWN,
|
||||
STAT_UNKNOWN,
|
||||
STAT_UNKNOWN,
|
||||
STAT_CHECKSUM_FAIL,
|
||||
STAT_UNKNOWN,
|
||||
STAT_UNKNOWN,
|
||||
STAT_RMC,
|
||||
STAT_UNKNOWN
|
||||
};
|
||||
|
||||
static int err_cnt;
|
||||
|
||||
void
|
||||
callback(lwgps_statement_t res) {
|
||||
static int i;
|
||||
|
||||
if (res != expected[i]) {
|
||||
printf("failed i %d, expected res %d but received %d\n",
|
||||
i, expected[i], res);
|
||||
++err_cnt;
|
||||
}
|
||||
|
||||
++i;
|
||||
}
|
||||
|
||||
int
|
||||
main() {
|
||||
/* Init GPS */
|
||||
lwgps_init(&hgps);
|
||||
|
||||
/* Process all input data */
|
||||
lwgps_process(&hgps, gps_rx_data, strlen(gps_rx_data), callback);
|
||||
|
||||
return err_cnt;
|
||||
}
|
||||
|
|
@ -1,71 +0,0 @@
|
|||
/*
|
||||
* This example uses direct processing function,
|
||||
* to process dummy NMEA data from GPS receiver
|
||||
*/
|
||||
#include <string.h>
|
||||
#include <stdio.h>
|
||||
#include "lwgps/lwgps.h"
|
||||
#include "test_common.h"
|
||||
|
||||
/* GPS handle */
|
||||
lwgps_t hgps;
|
||||
|
||||
/**
|
||||
* \brief Dummy data from GPS receiver
|
||||
*/
|
||||
const char
|
||||
gps_rx_data[] = ""
|
||||
"$GPRMC,183729,A,3907.356,N,12102.482,W,000.0,360.0,080301,015.5,E*6F\r\n"
|
||||
"$GPGGA,183730,3907.356,N,12102.482,W,1,05,1.6,646.4,M,-24.1,M,,*75\r\n"
|
||||
"$GPGSA,A,3,02,,,07,,09,24,26,,,,,1.6,1.6,1.0*3D\r\n"
|
||||
"$GPGSV,2,1,08,02,43,088,38,04,42,145,00,05,11,291,00,07,60,043,35*71\r\n"
|
||||
"$GPGSV,2,2,08,08,02,145,00,09,46,303,47,24,16,178,32,26,18,231,43*77\r\n"
|
||||
"";
|
||||
|
||||
/**
|
||||
* \brief Run the test of raw input data
|
||||
*/
|
||||
void
|
||||
run_tests() {
|
||||
lwgps_init(&hgps); /* Init GPS */
|
||||
|
||||
/* Process all input data */
|
||||
lwgps_process(&hgps, gps_rx_data, strlen(gps_rx_data));
|
||||
|
||||
/* Run the test */
|
||||
RUN_TEST(!INT_IS_EQUAL(hgps.is_valid, 0));
|
||||
RUN_TEST(INT_IS_EQUAL(hgps.fix, 1));
|
||||
RUN_TEST(INT_IS_EQUAL(hgps.fix_mode, 3));
|
||||
RUN_TEST(FLT_IS_EQUAL(hgps.latitude, 39.1226000000));
|
||||
RUN_TEST(FLT_IS_EQUAL(hgps.longitude, -121.0413666666));
|
||||
RUN_TEST(FLT_IS_EQUAL(hgps.altitude, 646.4000000000));
|
||||
RUN_TEST(FLT_IS_EQUAL(hgps.course, 360.0000000000));
|
||||
RUN_TEST(INT_IS_EQUAL(hgps.dop_p, 1.6000000000));
|
||||
RUN_TEST(INT_IS_EQUAL(hgps.dop_h, 1.6000000000));
|
||||
RUN_TEST(INT_IS_EQUAL(hgps.dop_v, 1.0000000000));
|
||||
RUN_TEST(FLT_IS_EQUAL(hgps.speed, 0.0000000000));
|
||||
RUN_TEST(FLT_IS_EQUAL(hgps.geo_sep, -24.100000000));
|
||||
RUN_TEST(FLT_IS_EQUAL(hgps.variation, 15.500000000));
|
||||
RUN_TEST(INT_IS_EQUAL(hgps.sats_in_view, 8));
|
||||
|
||||
RUN_TEST(INT_IS_EQUAL(hgps.sats_in_use, 5));
|
||||
RUN_TEST(INT_IS_EQUAL(hgps.satellites_ids[0], 2));
|
||||
RUN_TEST(INT_IS_EQUAL(hgps.satellites_ids[1], 0));
|
||||
RUN_TEST(INT_IS_EQUAL(hgps.satellites_ids[2], 0));
|
||||
RUN_TEST(INT_IS_EQUAL(hgps.satellites_ids[3], 7));
|
||||
RUN_TEST(INT_IS_EQUAL(hgps.satellites_ids[4], 0));
|
||||
RUN_TEST(INT_IS_EQUAL(hgps.satellites_ids[5], 9));
|
||||
RUN_TEST(INT_IS_EQUAL(hgps.satellites_ids[6], 24));
|
||||
RUN_TEST(INT_IS_EQUAL(hgps.satellites_ids[7], 26));
|
||||
RUN_TEST(INT_IS_EQUAL(hgps.satellites_ids[8], 0));
|
||||
RUN_TEST(INT_IS_EQUAL(hgps.satellites_ids[9], 0));
|
||||
RUN_TEST(INT_IS_EQUAL(hgps.satellites_ids[10], 0));
|
||||
RUN_TEST(INT_IS_EQUAL(hgps.satellites_ids[11], 0));
|
||||
|
||||
RUN_TEST(INT_IS_EQUAL(hgps.date, 8));
|
||||
RUN_TEST(INT_IS_EQUAL(hgps.month, 3));
|
||||
RUN_TEST(INT_IS_EQUAL(hgps.year, 1));
|
||||
RUN_TEST(INT_IS_EQUAL(hgps.hours, 18));
|
||||
RUN_TEST(INT_IS_EQUAL(hgps.minutes, 37));
|
||||
RUN_TEST(INT_IS_EQUAL(hgps.seconds, 30));
|
||||
}
|
||||
|
|
@ -1,18 +0,0 @@
|
|||
#ifndef TEST_COMMON_HDR_H
|
||||
#define TEST_COMMON_HDR_H
|
||||
|
||||
#include <math.h>
|
||||
#include <stdlib.h>
|
||||
|
||||
#define RUN_TEST(x) do { \
|
||||
if ((x)) { \
|
||||
printf("Test passed on line %u with condition " # x "\r\n", (unsigned)__LINE__); \
|
||||
} else { \
|
||||
printf("Test FAILED on line %u with condition " # x "\r\n", (unsigned)__LINE__ ); \
|
||||
exit(1); \
|
||||
} \
|
||||
} while (0)
|
||||
#define FLT_IS_EQUAL(x, y) (fabs((double)(x) - (double)(y)) < 0.00001)
|
||||
#define INT_IS_EQUAL(x, y) ((int)((x) == (y)))
|
||||
|
||||
#endif /* TEST_COMMON_HDR_H */
|
||||
|
|
@ -1,69 +0,0 @@
|
|||
/*
|
||||
* This example uses direct processing function,
|
||||
* to process dummy PUBX TIME packets from GPS receiver
|
||||
*/
|
||||
#include <string.h>
|
||||
#include <stdio.h>
|
||||
#include "lwgps/lwgps.h"
|
||||
#include "test_common.h"
|
||||
|
||||
#if !LWGPS_CFG_STATEMENT_PUBX_TIME
|
||||
#error "this test must be compiled with -DLWGPS_CFG_STATEMENT_PUBX_TIME=1"
|
||||
#endif /* !LWGPS_CFG_STATEMENT_PUBX_TIME */
|
||||
|
||||
/* GPS handle */
|
||||
lwgps_t hgps;
|
||||
|
||||
/**
|
||||
* \brief Dummy data from GPS receiver
|
||||
*/
|
||||
const char
|
||||
gps_rx_data_A[] = ""
|
||||
"$PUBX,04*37\r\n"
|
||||
"$PUBX,04,073731.00,091202,113851.00,1196,15D,1930035,-2660.664,43*71\r\n"
|
||||
"";
|
||||
const char
|
||||
gps_rx_data_B[] = ""
|
||||
"$PUBX,04,200714.00,230320,158834.00,2098,18,536057,257.043,16*12\r\b"
|
||||
"";
|
||||
|
||||
|
||||
/**
|
||||
* \brief Run the test of raw input data
|
||||
*/
|
||||
void
|
||||
run_tests() {
|
||||
lwgps_init(&hgps);
|
||||
|
||||
/* Process and test block A */
|
||||
lwgps_process(&hgps, gps_rx_data_A, strlen(gps_rx_data_A));
|
||||
|
||||
RUN_TEST(INT_IS_EQUAL(hgps.hours, 7));
|
||||
RUN_TEST(INT_IS_EQUAL(hgps.minutes, 37));
|
||||
RUN_TEST(INT_IS_EQUAL(hgps.seconds, 31));
|
||||
RUN_TEST(INT_IS_EQUAL(hgps.date, 9));
|
||||
RUN_TEST(INT_IS_EQUAL(hgps.month, 12));
|
||||
RUN_TEST(INT_IS_EQUAL(hgps.year, 2));
|
||||
RUN_TEST(FLT_IS_EQUAL(hgps.utc_tow, 113851.00));
|
||||
RUN_TEST(INT_IS_EQUAL(hgps.utc_wk, 1196));
|
||||
RUN_TEST(INT_IS_EQUAL(hgps.leap_sec, 15));
|
||||
RUN_TEST(INT_IS_EQUAL(hgps.clk_bias, 1930035));
|
||||
RUN_TEST(FLT_IS_EQUAL(hgps.clk_drift, -2660.664));
|
||||
RUN_TEST(INT_IS_EQUAL(hgps.tp_gran, 43));
|
||||
|
||||
/* Process and test block B */
|
||||
lwgps_process(&hgps, gps_rx_data_B, strlen(gps_rx_data_B));
|
||||
|
||||
RUN_TEST(INT_IS_EQUAL(hgps.hours, 20));
|
||||
RUN_TEST(INT_IS_EQUAL(hgps.minutes, 7));
|
||||
RUN_TEST(INT_IS_EQUAL(hgps.seconds, 14));
|
||||
RUN_TEST(INT_IS_EQUAL(hgps.date, 23));
|
||||
RUN_TEST(INT_IS_EQUAL(hgps.month, 3));
|
||||
RUN_TEST(INT_IS_EQUAL(hgps.year, 20));
|
||||
RUN_TEST(FLT_IS_EQUAL(hgps.utc_tow, 158834.00));
|
||||
RUN_TEST(INT_IS_EQUAL(hgps.utc_wk, 2098));
|
||||
RUN_TEST(INT_IS_EQUAL(hgps.leap_sec, 18));
|
||||
RUN_TEST(INT_IS_EQUAL(hgps.clk_bias, 536057));
|
||||
RUN_TEST(FLT_IS_EQUAL(hgps.clk_drift, 257.043));
|
||||
RUN_TEST(INT_IS_EQUAL(hgps.tp_gran, 16));
|
||||
}
|
||||
|
|
@ -1 +0,0 @@
|
|||
add_subdirectory(src)
|
||||
|
|
@ -1,3 +0,0 @@
|
|||
add_subdirectory(include)
|
||||
add_subdirectory(lwgps)
|
||||
|
||||
|
|
@ -1,7 +0,0 @@
|
|||
target_include_directories(${LIB_LWGPS_NAME} INTERFACE
|
||||
${CMAKE_CURRENT_SOURCE_DIR}
|
||||
)
|
||||
|
||||
target_include_directories(${LIB_LWGPS_NAME} PRIVATE
|
||||
${CMAKE_CURRENT_SOURCE_DIR}
|
||||
)
|
||||
|
|
@ -1,3 +0,0 @@
|
|||
target_sources(${LIB_LWGPS_NAME} PRIVATE
|
||||
lwgps.c
|
||||
)
|
||||
Some files were not shown because too many files have changed in this diff Show More
Loading…
Reference in New Issue