tuber/thirdparty_libs/ssd1327_ft/Src/gps/gps.c

/*
 * gps.c
 *
 *  Created on: Nov 15, 2019
 *      Author: Bulanov Konstantin
 *
 *  Contact information
 *  -------------------
 *
 * e-mail   :  leech001@gmail.com
 */

/*
 * |---------------------------------------------------------------------------------
 * | Copyright (C) Bulanov Konstantin,2019
 * |
 * | This program is free software: you can redistribute it and/or modify
 * | it under the terms of the GNU General Public License as published by
 * | the Free Software Foundation, either version 3 of the License, or
 * | any later version.
 * |
 * | This program is distributed in the hope that it will be useful,
 * | but WITHOUT ANY WARRANTY; without even the implied warranty of
 * | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * | GNU General Public License for more details.
 * |
 * | You should have received a copy of the GNU General Public License
 * | along with this program.  If not, see <http://www.gnu.org/licenses/>.
 * |---------------------------------------------------------------------------------
 */


#include <stdio.h>
#include <string.h>
//#include <usart.h>
#include "gps.h"
#include "../admisc/ad0x0_timman.h"
#include "stm32f1xx_hal_uart.h"
#include <cstdlib>

#if (GPS_DEBUG == 1)
#include <usbd_cdc_if.h>
#endif

uint8_t rx_data = 0;
uint8_t rx_buffer[GPSBUFSIZE];
uint8_t rx_index = 0;
UART_HandleTypeDef *p_huart;
uint32_t last_recieve_tick=0;
uint32_t last_HAL_res=0;
HAL_UART_StateTypeDef last_UART_state;
uint8_t	gps_enable=0;
uint8_t	isinint=0;

static GPS_t GPS;
static gps_data_t gps_data;//для рф24
void (*gps_data_ready_func)(gps_data_t*);
	
static void	update_gps_data(void);
uint8_t gps_wakeup(void);
static void	gps_err(void){
	__ASM("nop");
}

#if (GPS_DEBUG == 1)
void GPS_print(char *data){
	char buf[GPSBUFSIZE] = {0,};
	sprintf(buf, "%s\n", data);
	CDC_Transmit_FS((unsigned char *) buf, (uint16_t) strlen(buf));
}
#endif
static uint32_t get_period_ms(uint32_t _tick_start){
	uint32_t t=(HAL_GetTick()<_tick_start)
		?((uint64_t)HAL_GetTick()+UINT32_MAX-_tick_start)
		:(HAL_GetTick()-_tick_start);
	return t;
}

void GPS_Init(UART_HandleTypeDef *_huart,void (*p_data_ready_func)(gps_data_t*))
{
	p_huart=_huart;
	gps_data_ready_func=p_data_ready_func;
	ad0x0_timman_remove(gps_wakeup);
	ad0x0_timman_add(100,gps_wakeup);
	//HAL_UART_Receive_IT(p_huart, &rx_data, 1);// перенес в старт
}

void GPS_UART_CallBack(void){
	if(isinint){
		ad0x0_err();
		return;
	}
	isinint=true;
	if (rx_data != '\n' && rx_index < sizeof(rx_buffer)) {
		rx_buffer[rx_index++] = rx_data;
	} else {

		#if (GPS_DEBUG == 1)
		GPS_print((char*)rx_buffer);
		#endif

		if(GPS_validate((char*) rx_buffer)){
//			GPS_parse((char*) rx_buffer);
			GPS_parse_ad0x0((char*) rx_buffer);
		}
		rx_index = 0;
		memset(rx_buffer, 0, sizeof(rx_buffer));
	}
	
	//last_HAL_res=HAL_UART_Receive_IT(p_huart, &rx_data, 1);
	last_HAL_res=HAL_UART_GetError(p_huart);
	last_UART_state=HAL_UART_GetState(p_huart);	
	if(HAL_OK==last_HAL_res){		
		if(HAL_UART_STATE_READY!=last_UART_state){
			__ASM("nop");
		}else{
			last_HAL_res=HAL_UART_Receive_IT(p_huart, &rx_data, 1);
		}
	}
	last_recieve_tick=HAL_GetTick();
	isinint=false;
}

void GPS_AbortReceiveCpltCallback(void){
	gps_start();
}

uint8_t gps_retransmit(void){
	last_UART_state=HAL_UART_GetState(p_huart);
	if(last_UART_state==HAL_UART_STATE_READY){
		last_HAL_res=HAL_UART_Receive_IT(p_huart, &rx_data, 1);
	}else{
		if(last_UART_state==HAL_UART_STATE_RESET){
			gps_enable=false;
			last_UART_state=HAL_UART_GetState(p_huart);
			last_HAL_res=HAL_UART_AbortReceive_IT(p_huart);
			return 0;
//			gps_enable=true;
		}
		if(last_UART_state & HAL_UART_ERROR_ORE){
			__HAL_UART_CLEAR_OREFLAG(p_huart);
			uint8_t tmpdr=p_huart->Instance->DR;
			last_UART_state=HAL_UART_GetState(p_huart);
		}
		if(last_UART_state & HAL_UART_ERROR_FE){
			__HAL_UART_CLEAR_FEFLAG(p_huart);
			last_UART_state=HAL_UART_GetState(p_huart);
			gps_enable=false;
			last_UART_state=HAL_UART_GetState(p_huart);
			last_HAL_res=HAL_UART_AbortReceive_IT(p_huart);
		}
		if(last_UART_state & HAL_UART_ERROR_NE){
			__HAL_UART_CLEAR_NEFLAG(p_huart);
			last_UART_state=HAL_UART_GetState(p_huart);
		}
		last_UART_state=HAL_UART_GetState(p_huart);
		if(last_UART_state==HAL_UART_STATE_READY){
			last_HAL_res=HAL_UART_Receive_IT(p_huart, &rx_data, 1);
		}
	}
	//if(HAL_OK!=last_HAL_res){		gps_err();	}
//	last_recieve_tick=HAL_GetTick();
//	ad0x0_timman_remove(gps_retransmit);
//	ad0x0_timman_add(10,gps_retransmit);	
	return 1;
}

void gps_start(void){
	gps_enable=true;
	gps_retransmit();
}

void GPS_UART_ErrorCallback(void){
	last_HAL_res=HAL_UART_GetError(p_huart);
	last_UART_state=HAL_UART_GetState(p_huart);
	if(last_UART_state & HAL_UART_ERROR_ORE){
		__HAL_UART_CLEAR_OREFLAG(p_huart);
	}
	//!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
	last_UART_state=HAL_UART_GetState(p_huart);
	if(HAL_UART_STATE_READY!=last_UART_state){
			__ASM("nop");
	}else{
			last_HAL_res=HAL_UART_Receive_IT(p_huart, &rx_data, 1);
	}
/*	switch(last_HAL_res){
//		case HAL_UART_ERROR_NONE: // – без ошибок;
		case HAL_UART_ERROR_PE: // – ошибка целостности данных;
			ad0x0_err();
		break;
		case HAL_UART_ERROR_NE: // – выделен шум;
			ad0x0_err();
		break;
		case HAL_UART_ERROR_FE: // – ошибка фрагментации;
			ad0x0_err();
		break;
		case HAL_UART_ERROR_ORE: // – ошибка переполнения;
			__HAL_UART_CLEAR_OREFLAG(p_huart);
			last_HAL_res=HAL_UART_Receive_IT(p_huart, &rx_data, 1);
		break;
		case HAL_UART_ERROR_DMA: // – ошибка DMA.
			ad0x0_err();
		break;
		//	ad0x0_timman_remove(gps_retransmit);
//			ad0x0_timman_add(100,gps_retransmit);
		default:
			ad0x0_err();
			last_HAL_res=HAL_UART_Receive_IT(p_huart, &rx_data, 1);
			if(HAL_OK!=last_HAL_res){
				gps_err();
			}
	}*/
}

uint8_t gps_wakeup(void){
	
	uint32_t delta=get_period_ms(last_recieve_tick);
	if(delta>1000){
		if(gps_enable){
//			HAL_UART_DeInit(p_huart);
//			HAL_UART_Init(p_huart);
			last_HAL_res=HAL_UART_AbortReceive_IT(p_huart);
			gps_retransmit();
			last_recieve_tick=HAL_GetTick();
		}
	}
	return 0;
}

int GPS_validate(char *nmeastr){
    char check[3];
    char checkcalcstr[3];
    int i;
    int calculated_check;

    i=0;
    calculated_check=0;

    // check to ensure that the string starts with a $
    if(nmeastr[i] == '$')
        i++;
    else
        return 0;

    //No NULL reached, 75 char largest possible NMEA message, no '*' reached
    while((nmeastr[i] != 0) && (nmeastr[i] != '*') && (i < 75)){
        calculated_check ^= nmeastr[i];// calculate the checksum
        i++;
    }

    if(i >= 75){
        return 0;// the string was too long so return an error
    }

    if (nmeastr[i] == '*'){
        check[0] = nmeastr[i+1];    //put hex chars in check string
        check[1] = nmeastr[i+2];
        check[2] = 0;
    }
    else
        return 0;// no checksum separator found there for invalid

    sprintf(checkcalcstr,"%02X",calculated_check);
    return((checkcalcstr[0] == check[0])
        && (checkcalcstr[1] == check[1])) ? 1 : 0 ;
}

void	setval(char _pt,void *p,char *str_p){
	/*ну да, б, заморочился с парсером, ваще не хотел его писАть, но решил задрочить себе мозги*/
	int d=0xFFFF;
	float f;
	if(!str_p){
		ad0x0_err();
		return;
	}
	switch(_pt){
		case 's':
			if(p)strcpy((char*)p,str_p);
		break;
		case 'c':
			if(p)*((char*)p)=*str_p;
		break;
		case 'f':
			f=atof(str_p);
			if(p)*((float*)p)=f;
		break;
		case 'd':
			sscanf(str_p,"%d",&d);
			if(p)*((int*)p)=d;
		break;
		default:
			gps_err();
			ad0x0_err();
	}
}

void parse_g(char *s,char *t,void **p){
	char b[32],*pb;
	uint8_t i=0;
	pb=b;
	if(!s){
		ad0x0_err();
		return;
	}
	memset(b,'x',sizeof(b));
	while((i<75)&&(*s!='*')&&(*s)&&(*t)){
		if(*s==','){
			*pb++=0;
			if(*t && *b)setval(*t,*p,b);
			pb=b;
			p++;
			t++;
		}else{
			*pb++=*s;
		}
		s++;
		i++;
	}	
	
}

void GPS_parse_ad0x0(char *s){	
	//$GNRMC,180159.00,A,5137.81278,N,03815.19037,E,2.468,71.94,050119,,,A*4C
	if(!strncmp(s, "$GNRMC", 6)){//ad0x0
		char t[]="sfcfcfcffd\0";
		void *p[]={NULL,&GPS.utc_time, NULL, &GPS.nmea_latitude, &GPS.ns, &GPS.nmea_longitude, &GPS.ew, &GPS.speed_k, &GPS.course_d, &GPS.date};
		parse_g(s,t,p);
		GPS.dec_latitude = GPS_nmea_to_dec(GPS.nmea_latitude, GPS.ns);
		GPS.dec_longitude = GPS_nmea_to_dec(GPS.nmea_longitude, GPS.ew);
		update_gps_data();
	}else
	if(!strncmp(s, "$GNGGA", 6)){
		char t[]="sffcfcddffc\0";
		void *p[]={NULL,&GPS.utc_time, &GPS.nmea_latitude, &GPS.ns, &GPS.nmea_longitude, &GPS.ew, &GPS.lock, &GPS.satelites, &GPS.hdop, &GPS.msl_altitude, &GPS.msl_units};
		parse_g(s,t,p);
		GPS.dec_latitude = GPS_nmea_to_dec(GPS.nmea_latitude, GPS.ns);
		GPS.dec_longitude = GPS_nmea_to_dec(GPS.nmea_longitude, GPS.ew);
		update_gps_data();
  }
		
	
}

void GPS_parse(char *GPSstrParse){	

	char m;		
		if (!strncmp(GPSstrParse, "$GNRMC", 6)){//ad0x0
			//$GNRMC,180159.00,A,5137.81278,N,03815.19037,E,2.468,71.94,050119,,,A*4C
    	if(sscanf(GPSstrParse, "$GNRMC,%f,%c,%f,%c,%f,%c,%f,%f,%d", &GPS.utc_time, &m, &GPS.nmea_latitude, &GPS.ns, &GPS.nmea_longitude, &GPS.ew, &GPS.speed_k, &GPS.course_d, &GPS.date) >= 1){
				GPS.dec_latitude = GPS_nmea_to_dec(GPS.nmea_latitude, GPS.ns);
    		GPS.dec_longitude = GPS_nmea_to_dec(GPS.nmea_longitude, GPS.ew);
				update_gps_data();
    		return;
		}
    }else 
		if(!strncmp(GPSstrParse, "$GNGGA", 6)){
    	if (sscanf(GPSstrParse, "$GNGGA,%f,%f,%c,%f,%c,%d,%d,%f,%f,%c", &GPS.utc_time, &GPS.nmea_latitude, &GPS.ns, &GPS.nmea_longitude, &GPS.ew, &GPS.lock, &GPS.satelites, &GPS.hdop, &GPS.msl_altitude, &GPS.msl_units) >= 1){
    		GPS.dec_latitude = GPS_nmea_to_dec(GPS.nmea_latitude, GPS.ns);
    		GPS.dec_longitude = GPS_nmea_to_dec(GPS.nmea_longitude, GPS.ew);
				update_gps_data();
    		return;
    	}
    }else
    if(!strncmp(GPSstrParse, "$GPGGA", 6)){
    	if (sscanf(GPSstrParse, "$GPGGA,%f,%f,%c,%f,%c,%d,%d,%f,%f,%c", &GPS.utc_time, &GPS.nmea_latitude, &GPS.ns, &GPS.nmea_longitude, &GPS.ew, &GPS.lock, &GPS.satelites, &GPS.hdop, &GPS.msl_altitude, &GPS.msl_units) >= 1){
    		GPS.dec_latitude = GPS_nmea_to_dec(GPS.nmea_latitude, GPS.ns);
    		GPS.dec_longitude = GPS_nmea_to_dec(GPS.nmea_longitude, GPS.ew);
				update_gps_data();
    		return;
    	}
    }
    else if (!strncmp(GPSstrParse, "$GPRMC", 6)){
    	if(sscanf(GPSstrParse, "$GPRMC,%f,%f,%c,%f,%c,%f,%f,%d", &GPS.utc_time, &GPS.nmea_latitude, &GPS.ns, &GPS.nmea_longitude, &GPS.ew, &GPS.speed_k, &GPS.course_d, &GPS.date) >= 1){
				update_gps_data();
    		return;
		}

    }
    else if (!strncmp(GPSstrParse, "$GPGLL", 6)){
        if(sscanf(GPSstrParse, "$GPGLL,%f,%c,%f,%c,%f,%c", &GPS.nmea_latitude, &GPS.ns, &GPS.nmea_longitude, &GPS.ew, &GPS.utc_time, &GPS.gll_status) >= 1){
					update_gps_data();
          return;
			}
    }
    else if (!strncmp(GPSstrParse, "$GPVTG", 6)){
        if(sscanf(GPSstrParse, "$GPVTG,%f,%c,%f,%c,%f,%c,%f,%c", &GPS.course_t, &GPS.course_t_unit, &GPS.course_m, &GPS.course_m_unit, &GPS.speed_k, &GPS.speed_k_unit, &GPS.speed_km, &GPS.speed_km_unit) >= 1)
					update_gps_data();
            return;
    }
}

float GPS_nmea_to_dec(float deg_coord, char nsew) {
    int degree = (int)(deg_coord/100);
    float minutes = deg_coord - degree*100;
    float dec_deg = minutes / 60;
    float decimal = degree + dec_deg;
    if (nsew == 'S' || nsew == 'W') { // return negative
        decimal *= -1;
    }
    return decimal;
}

void	update_gps_data(void){
			gps_data.latitude=GPS.dec_latitude;
			gps_data.longitude=GPS.dec_longitude;
			gps_data.satelites=GPS.satelites;
			gps_data.speed_km=GPS.utc_time;
		if(gps_data_ready_func)gps_data_ready_func(&gps_data);
}