4G/code/app/AppTaskGps.c

/*
 * @Author: chenjie
 * @Date: 2022-06-06
 * @LastEditTime: 2022-10-27
 * @LastEditors: chenjie
 * @Description: 
 * @FilePath: \undefinedd:\1_WorkFiles\1_Device\2_4G_G1\2_SW\workspace\S32K146_4G\code\app\AppTaskGps.c
 * Copyright (c) 2022 by chenjie, All Rights Reserved.
 */

#include "AppTaskGps.h"
void strdel(char *str, char c);
uint32 location_handle(char *in1);
void GpsDataDecode(uint8 *msg);
uint8 GpsRecvPtr[1024];
uint32 timerCounterGetdata = 0;
uint32 ledCounterTmp = 0;
uint32 triaxiCounterTmp = 0;
static void vtimerCallback(TimerHandle_t pxTimer)
{
	uint32 ulTimerID;
	ulTimerID = (uint32)pvTimerGetTimerID(pxTimer);
	if (ulTimerID == 0)
	{
		timerCounterGetdata++;
	}
}
void GpsTask(void *pvParameters)
{
	(void)pvParameters;
	GpsDataQueueHandle = xQueueCreate(1, sizeof(GPSInfo));//长度为1才可以允许覆写
	Dio_WriteChannel(DioConf_DioChannel_PTD1_GPIO_OUT_MCU_GPS_POW_EN, STD_ON);//GPS开机
	uint16 pReadLen = 0;
	TimerHandle_t monitorTimer1ms;
	monitorTimer1ms = xTimerCreate("monitor1ms", 1, pdTRUE, (void *)0, vtimerCallback);
	xTimerStart(monitorTimer1ms, 0);
	real_T S0[SIZE_FFT];
	real_T S1[SIZE_FFT];
	real_T S2[SIZE_FFT];
	uint8 fftIdx = 0;
	uint16 GpsErrCounter = 0;
	uint16 GpsRestartCounter = 0;
	bool GpsPwrDownFlg = false;
	while(1)
	{
		memset(GpsRecvPtr,0,sizeof(GpsRecvPtr));
		UART_Receive_Data(UART_LPUART2,GpsRecvPtr,&pReadLen,1);
		if(pReadLen>0)
		{
			GpsDataDecode(GpsRecvPtr);
		}
		if(TimerCounter%1000==0)
		{
			if(DeviceSpeed>0)//GPS定位成功
			{
				GpsErrCounter = 0;
			}
			else
			{
				if(GpsErrCounter>60*10)
				{
					Dio_WriteChannel(DioConf_DioChannel_PTD1_GPIO_OUT_MCU_GPS_POW_EN, STD_OFF);//GPS关机
					GpsErrCounter = 0;
					GpsPwrDownFlg = true;
					GpsRestartCounter = 0;
				}
				if(GpsPwrDownFlg)
				{
					GpsRestartCounter+=1;
				}
				GpsErrCounter+=1;
			}
			if(GpsRestartCounter>10)
			{
				GpsRestartCounter = 0;
				GpsPwrDownFlg = false;
				Dio_WriteChannel(DioConf_DioChannel_PTD1_GPIO_OUT_MCU_GPS_POW_EN, STD_ON);//GPS开机
			}
		}
		/*LED控制*/
		if((timerCounterGetdata - ledCounterTmp) > 200)
		{
			Dio_WriteChannel(LED_INDEX2, STD_OFF);
			ledCounterTmp = timerCounterGetdata;
			if(SocketId>=0)//网络连接成功
			{
				Dio_FlipChannel(LED_INDEX3);
			}
			else
			{
				Dio_WriteChannel(LED_INDEX3,STD_OFF);
			}
			if(DeviceSpeed>0)//GPS定位成功
			{
				Dio_FlipChannel(LED_INDEX4);
				GpsErrCounter = 0;
			}
			else
			{
				Dio_WriteChannel(LED_INDEX4,STD_OFF);
			}
		}
		/*三轴计算*/
		if ((timerCounterGetdata - triaxiCounterTmp)> 10)
		{
			triaxiCounterTmp = timerCounterGetdata;
			SL_SC7A20_Read_XYZ_Data(xyzData);
			S0[fftIdx] = (real_T)xyzData[0];
			S1[fftIdx] = (real_T)xyzData[1];
			S2[fftIdx] = (real_T)xyzData[2];
			fftIdx++;
		}
		if (fftIdx >= SIZE_FFT)
		{
			fftIdx = 0;
			fft(S0, sizeof(S0) / sizeof(real_T), 100, returnFreq[0], returnP[0]); // 50 max
			fft(S1, sizeof(S1) / sizeof(real_T), 100, returnFreq[1], returnP[1]);
			fft(S2, sizeof(S2) / sizeof(real_T), 100, returnFreq[2], returnP[2]);
		}
	}
}
void GpsDataDecode(uint8 *msg)
{
//	char temp[] = "$GNRMC,082626.000,A,2939.91801,N,10637.09500,E,5.543,30.254,261120,,,A,V*17";
	char *p = NULL;
	char  *p2 = NULL;
	const char *delim = "\n";
	GPSInfo GpsInfoData;
	uint8 index = 0;
	char *databuffer[20];
	uint32 speedtemp;
	uint32 latitude;
	uint32 longitude;
	uint16 direction;
	memset((uint8 *)&GpsInfoData, 0x00, sizeof(GPSInfo));
	p = strtok(msg, delim); //将信息进行分割
	p2 = strtok(NULL, delim);
	p = strtok(p, ","); //只取第1行的信息RMC
//	p = strtok(temp,",");//模拟测试
	if (strcmp(p, "$GNRMC") == 0)
	{
		index = 0;
		while (p)
		{
			databuffer[index] = p;
			p = strtok(NULL, ",");
			index++;
		}
		if (index > 5 && strcmp(databuffer[2], "A") == 0)
		{
			GpsInfoData.locateMark = 0x01; //有效，东经，北纬写定
			strdel(databuffer[3], '.');
			strdel(databuffer[5], '.');
			strdel(databuffer[7], '.');
			speedtemp = atol(databuffer[7]) * 1852 / 1e5; //节换算单位，1节=1.852km每小时
			DeviceSpeed = speedtemp+1;
			latitude = location_handle(databuffer[3]);
			GpsInfoData.latitude[0] = latitude >> 24;
			GpsInfoData.latitude[1] = latitude >> 16;
			GpsInfoData.latitude[2] = latitude >> 8;
			GpsInfoData.latitude[3] = latitude;
			longitude = location_handle(databuffer[5]);
			GpsInfoData.longitude[0] = longitude >> 24;
			GpsInfoData.longitude[1] = longitude >> 16;
			GpsInfoData.longitude[2] = longitude >> 8;
			GpsInfoData.longitude[3] = longitude;
			if (speedtemp >= 50) //大于5km/h才输出方位
			{
				direction = atol(databuffer[8]);
				GpsInfoData.direction[0] = direction >> 8;
				GpsInfoData.direction[1] = direction;
				GpsInfoData.speed[0] = (speedtemp >> 8) & 0xFF;
				GpsInfoData.speed[1] = speedtemp & 0xFF;
			}
			else
			{
				GpsInfoData.direction[0] = 0;
				GpsInfoData.direction[1] = 0;
				GpsInfoData.speed[0] = 0;
				GpsInfoData.speed[1] = 0;
			}
			if (speedtemp >= 50 && speedtemp <= 1500)
			{
				AppConfigInfo.appSaveFlg = true;
				AppConfigInfo.AppDataInfo.AccMileage = speedtemp / 36 + AppConfigInfo.AppDataInfo.AccMileage;
				if (AppConfigInfo.AppDataInfo.AccMileage >= 0xfffffffe)
				{
					AppConfigInfo.AppDataInfo.AccMileage = 0;
				}
			}//累计里程的累加
		}
		else
		{
			DeviceSpeed = 0;
		}
		GpsInfoData.AccMileage[0] = AppConfigInfo.AppDataInfo.AccMileage >> 24;
		GpsInfoData.AccMileage[1] = AppConfigInfo.AppDataInfo.AccMileage >> 16;
		GpsInfoData.AccMileage[2] = AppConfigInfo.AppDataInfo.AccMileage >> 8;
		GpsInfoData.AccMileage[3] = AppConfigInfo.AppDataInfo.AccMileage;
	}
	p2 = strtok(p2, ","); //只取第2行的信息GGA
	memset(databuffer, 0x30, 20);
	if (strcmp(p2, "$GNGGA") == 0)
	{
		index = 0;
		while (p2)
		{
			databuffer[index] = p2;
			p2 = strtok(NULL, ",");
			index++;
		}
		if (index > 9 && (strcmp(databuffer[6], "1") == 0 || strcmp(databuffer[6], "2") == 0))
		{
			GpsInfoData.satelliteNum = atol(databuffer[7]); //卫星数目写入
			strdel(databuffer[9], '.');
			uint16 alt = 0;
			alt = atol(databuffer[9]) / 10 + 1000;
			GpsInfoData.altitude[0] = alt >> 8; //海拔
			GpsInfoData.altitude[1] = alt;
			strdel(databuffer[2], '.');
			strdel(databuffer[4], '.');
			latitude = location_handle(databuffer[2]);
			GpsInfoData.latitude[0] = latitude >> 24;
			GpsInfoData.latitude[1] = latitude >> 16;
			GpsInfoData.latitude[2] = latitude >> 8;
			GpsInfoData.latitude[3] = latitude;
			longitude = location_handle(databuffer[4]);
			GpsInfoData.longitude[0] = longitude >> 24;
			GpsInfoData.longitude[1] = longitude >> 16;
			GpsInfoData.longitude[2] = longitude >> 8;
			GpsInfoData.longitude[3] = longitude;
		}
	}
	xQueueOverwrite(GpsDataQueueHandle,(uint8 *)&GpsInfoData);
}
void strdel(char *str, char c)
{
	char *p = str;
	while (*str)
	{
		if (*str != c)
			*p++ = *str;
		str++;
	}
	*p = '\0';
}
uint32 location_handle(char *in1)
{
	uint32 location_temp;
	uint32 location_degree;
	uint32 location_min;
	location_temp = atol(in1);
	location_degree = location_temp / (1e7);
	location_degree = location_degree * (1e6);
	location_min = location_temp - location_degree * 10;
	location_min = location_min / 6;
	location_temp = location_degree + location_min;
	return location_temp;
}