vehicle-backup/zw_pro/EgoSystem/Rtk.cpp

#include <stdint.h>
#include <stdlib.h>
#include "../common/comm.h"
#include "../common/iobuffer.h"
#include "../common/sensor_socket.h"


#include "Rtk.h"
#include "message_queue.h"
#include "MqttClient.h"
#include "imu_sensor.h"

#include <string.h>
#include <sys/time.h>


CRtkSensor::CRtkSensor(CMessageQueue* q) :_message(q)
{
	F_SendDate = false;
	_run = false;
}



gtime_t CRtkSensor::timeadd(gtime_t t, double sec)
{
    double tt;
    
    t.sec+=sec; tt=floor(t.sec); t.time+=(int)tt; t.sec-=tt;
    return t;
}

double CRtkSensor::timediff(gtime_t t1, gtime_t t2)
{
    return difftime(t1.time,t2.time)+t1.sec-t2.sec;
}

gtime_t CRtkSensor::epoch2time(const double *ep)
{
    const int doy[]={1,32,60,91,121,152,182,213,244,274,305,335};
    gtime_t time={0};
    int days,sec,year=(int)ep[0],mon=(int)ep[1],day=(int)ep[2];
    
    if (year<1970||2099<year||mon<1||12<mon) return time;
    
    /* leap year if year%4==0 in 1901-2099 */
    days=(year-1970)*365+(year-1969)/4+doy[mon-1]+day-2+(year%4==0&&mon>=3?1:0);
    sec=(int)floor(ep[5]);
    time.time=(time_t)days*86400+(int)ep[3]*3600+(int)ep[4]*60+sec;
    time.sec=ep[5]-sec;
    return time;
}

//gps 转utc
gtime_t CRtkSensor::gpst2utc(gtime_t t)
{
    gtime_t tu;
    int i;
    
    for (i=0;leaps[i][0]>0;i++) {
        tu=timeadd(t,leaps[i][6]);
        if (timediff(tu,epoch2time(leaps[i]))>=0.0) return tu;
    }
    return t;
}



void CRtkSensor::Notify(int8_t* buffer, int32_t size)
{

	//$GPCHC,2319,183643.68,119.85, 1.67,2.53,0.02,-0.13,-0.06,-0.0450,0.0282,1.0009,21.06074840,110.50436901,-1.54,-0.002,0.007,0.008,50,46,42,1,2*40


    //$GPCHC,1980,4632.64,289.19,-0.42,0.21,-0.23,0.07,-0.06,
	//      p    p1      p      p1    p    p1    p    p1    p
    //0.0009,0.0048,-1.0037,38.8594969,121.5150073,121.51,-0.023,0.011,0.011,1.500,14,6,42,0,2*67
	//      p1     p      p1          p

	if(_run)
	{
		int Temp = ',';
		unsigned char* p = NULL, * p1 = NULL;
		p = (unsigned char*)strchr((const char*)buffer, Temp);  //header
		p1 = (unsigned char*)strchr((const char*)p + 1, Temp);  //Gpsweek
		float gpsweek = strtof((const char*)p1 + 1, NULL);
		p = (unsigned char*)strchr((const char*)p1 + 1, Temp);   //gpsTime
		float gpsTime =  strtof((const char*)p + 1, NULL);

		float headingAngle = strtof((const char*)p + 1, NULL); //heading
		//std::cout << "angle"  <<headingAngle << std::endl;

		//����IMU
		ImuData m_imu;
		p1 = (unsigned char*)strchr((const char*)p + 1, Temp);//pithch
		// m_imu.rx = strtof((const char*)p1 + 1, NULL);
		
		p = (unsigned char*)strchr((const char*)p1 + 1, Temp);//roll
		// m_imu.ry = strtof((const char*)p + 1, NULL);

		// _count++;

        // if (_count > 20)
        // {
        //     _message->WriteIMUData(&m_imu);
        //     //std::cout << "Send Date" << std::endl;
        //     _count = 0;
        // }
		

		
		p1 = (unsigned char*)strchr((const char*)p + 1, Temp);  //gyro x
		p = (unsigned char*)strchr((const char*)p1 + 1, Temp);  //gyro y
		p1 = (unsigned char*)strchr((const char*)p + 1, Temp);  //gyro z
		p = (unsigned char*)strchr((const char*)p1 + 1, Temp);  //acc x
		p1 = (unsigned char*)strchr((const char*)p + 1, Temp);  //acc y
		p = (unsigned char*)strchr((const char*)p1 + 1, Temp);  //acc z

		p1 = (unsigned char*)strchr((const char*)p + 1, Temp);   //lat
		float lat = strtof((const char*)p1 + 1, NULL);
		p = (unsigned char*)strchr((const char*)p1 + 1, Temp); //lon
		float lon = strtof((const char*)p + 1, NULL);

		//增加
		p1 = (unsigned char*)strchr((const char*)p + 1, Temp);  //alti
		p = (unsigned char*)strchr((const char*)p1 + 1, Temp); //Ve
		p1 = (unsigned char*)strchr((const char*)p + 1, Temp);  //Vn
		p = (unsigned char*)strchr((const char*)p1 + 1, Temp); //Vu
		p1 = (unsigned char*)strchr((const char*)p + 1, Temp);  //V
		p = (unsigned char*)strchr((const char*)p1 + 1, Temp); //NSV1
		p1 = (unsigned char*)strchr((const char*)p + 1, Temp);  //NSV2
		p = (unsigned char*)strchr((const char*)p1 + 1, Temp); //STATUS
		float loc_status = strtof((const char*)p + 1, NULL);    //
		//std::cout << "loc status" << loc_status << std::endl;
		m_imu.rx = strtof((const char*)p + 1, NULL);  //
		if(m_imu.rx == 42)
			m_imu.ry =1;
		else 
			m_imu.ry = 0;
		// m_imu.ry = strtof((const char*)p + 1, NULL);  //
		_message ->WriteIMUData((&m_imu));


		 
	

		
		/*_count++;

		if (_count > 10)
		{
			if(CMqttSensor::_run)
				Send_ZGJ_Rtk(headingAngle, lat, lon);
			_count = 0;
		}*/

		if (!F_SendDate)
		{
			_curTick = std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::system_clock::now().time_since_epoch()).count();
			F_SendDate = true;
		}
		else
		{
			long long tick = std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::system_clock::now().time_since_epoch()).count();
			if (_curTick != 0 && tick - _curTick > 300)
			{
				if (CMqttSensor::_run && (CMqttSensor::m_mqttClient != NULL))
					// Send_ZGJ_Rtk(headingAngle, lat, lon,loc_status);
					Send_ZGJ_Rtk(gpsweek,gpsTime,headingAngle, lat, lon,loc_status);
				F_SendDate = false;
			}
		}
	}
	
}


void CRtkSensor::Start()
{
	_run = true;
}


void CRtkSensor::Run()
{


}

void CRtkSensor::SetSensorSocket(SensorSocket<CRtkSensor>* udp)
{
    _socket = udp;
}
void CRtkSensor::Stop()
{
	F_SendDate = false;
	_run = false;
}

#include "../thirdparty/jsoncpp/include/json/json.h"

void CRtkSensor::Send_ZGJ_Rtk(float gpsweek,float gpsTime,float headingAngle,float lat,float lon,float loc_status)//ץ�ֻ�λ����Ϣ�ϱ�
{



// gtime_t CRtkSensor::gpst2utc(gtime_t t)
    

	struct timeval tv;
	gettimeofday(&tv, NULL);

	char m_Send[16],Dst[16];
	memset(Dst, 0, 16);

	// sprintf((char*)Dst, "%ld", tv.tv_sec * 1000 + tv.tv_usec / 1000);
	sprintf((char*)Dst, "%f", rtk_time.time * 1000 + rtk_time.sec);

	// gtime_t CRtkSensor::gpst2utc(gtime_t t);


	Json::Value root;
	Json::Value Source;
	Json::FastWriter writer;

	std::string SendTime;
	std::string m_headingAngle;
	std::string m_lon;
	std::string m_lat;
	std::string m_loc_status;

	Source["deviceEsn"] = "";
	Source["cloudEsn"] = CMqttSensor::_Esn;
	Source["sendTime"] = SendTime.assign(Dst,strlen(Dst));
	Source["ack"] = false;

	Source["seqNum"] = CMqttSensor::_Esn + SendTime.assign(Dst,strlen(Dst));

	Json::Value GraspGPS;
	GraspGPS["graspEsn"] = CMqttSensor::_Esn;

	memset(m_Send, 0, 16);
	sprintf((char*)m_Send, "%.7f", lon);
	GraspGPS["lon"] = m_lon.assign(m_Send);


	memset(m_Send, 0, 16);
	sprintf((char*)m_Send, "%.7f", lat);
	GraspGPS["lat"] = m_lat.assign(m_Send);

	memset(m_Send, 0, 16);
	sprintf((char*)m_Send, "%.2f", headingAngle);
	GraspGPS["headingAngle"] = m_headingAngle.assign(m_Send);
	GraspGPS["timeStamp"] = SendTime.assign(Dst,strlen(Dst));

	memset(m_Send, 0, 16);
	sprintf((char*)m_Send, "%.0f", loc_status);
	GraspGPS["loc_status"] = m_loc_status.assign(m_Send);

	Source["graspInfoList"].append(GraspGPS);

	Json::StyledWriter sw;
	//std::cout << sw.write(Source) << std::endl << std::endl;

	//CMqttSensor::sendMessage((char*)sw.write(Source).c_str(), 1, (char*)*(CMqttSensor::subTopic));
	char Topic[64];
	memset(Topic, 0, 64);
	sprintf(Topic, "v2x/v1/grasp/%s/info/up", CMqttSensor::_Esn.c_str());
	CMqttSensor::sendMessage((char*)sw.write(Source).c_str(), 0, (char*)Topic);
}

// void CRtkSensor::Send_ZGJ_status(float gpsweek, ,int Status)//ץ�ֻ�״̬��Ϣ�ϱ�
// void CRtkSensor::Send_ZGJ_status(float gpsweek,float gpsTime,int Status)
void CRtkSensor::Send_ZGJ_status(int Status)
{
	if (CMqttSensor::_run)
	{
		struct timeval tv;
		gettimeofday(&tv, NULL);

		
		rtk_time.time = gpsweek * 604800 + int(gpsTime);
		rtk_time.sec = gpsTime - int(gpsTime);

		char Dst[16];
		memset(Dst, 0, 16);
		// sprintf((char*)Dst, "%ld", tv.tv_sec * 1000 + tv.tv_usec / 1000);
		sprintf((char*)Dst, "%f", rtk_time.time*1000  + rtk_time.sec*1000);


		Json::Value root;
		Json::Value Source;
		Json::FastWriter writer;

		std::string SendTime;

		Source["deviceEsn"] = "";
		Source["cloudEsn"] = CMqttSensor::_Esn;
		Source["sendTime"] = SendTime.assign(Dst,strlen(Dst));
		Source["ack"] = true;

		Source["seqNum"] = CMqttSensor::_Esn + SendTime.assign(Dst,strlen(Dst));

		Json::Value GraspStatus;
		GraspStatus["graspEsn"] = CMqttSensor::_Esn;
		GraspStatus["status"] = Status;
		GraspStatus["timeStamp"] = SendTime.assign(Dst,strlen(Dst));

		Source["graspStatusList"].append(GraspStatus);

		Json::StyledWriter sw;
		//std::cout << sw.write(Source) << std::endl << std::endl;

		char Topic[64];
		memset(Topic, 0, 64);
		sprintf(Topic, "v2x/v1/grasp/%s/status/up", CMqttSensor::_Esn.c_str());
		CMqttSensor::sendMessage((char*)sw.write(Source).c_str(), 0, (char*)Topic);
	}
}