毫米波雷达数据处理得到运动目标

#include “StdAfx.h“
#include “RadarAnalys.h“
#include “Radar.H“
#include 
#include “canlib.h“

int flagnum=0;

CRadarAnalys::CRadarAnalys（void）
{
	for （int i=0;i<3;i++）
	{
		for （int j=0;j<64;j++）
		{

			Original_Data[i][j].angle=0;
			Original_Data[i][j].range=0;
			Original_Data[i][j].speed=0;
			Original_Data[i][j].position_x=0;
			Original_Data[i][j].position_y=0;

		}
	
	}
	Quiet = 0;                 // Defines the verbosity.
	seq =0;
}

CRadarAnalys::~CRadarAnalys（void）
{
}
void CRadarAnalys::Initialize（）
{
	Source=0;
	Bitrate=BAUD_1M;

	canInitializeLibrary（）; 
	hnd = canOpenChannel（Source  0）;  //  Channel # 
	stat = canSetBusParams（hnd Bitrate 0 0 0 0 0）; // Selected bit rate.
	stat = canBusOn（hnd）; 

}
void CRadarAnalys::GetRadarData_Online（CString FilePath）
{
	int Overrun;

	long id;
	unsigned int dlc flags;
	unsigned char msg[8];

	unsigned long rawdata;
	unsigned long rawdatahigh;
	unsigned long rawdatalow;
	unsigned long rawdata1;
	unsigned long rawdata2;
	unsigned long rawdata2temp;
	unsigned long rawdata3;
	unsigned long yawrate_temp1;
	unsigned long speed_temp1;
	unsigned long speed_temp2;
	long yawrate_temp2;

	DWORD time;
	long t timeFrac timeInt;
	unsigned int i;


	double ANGLE;
	double RANGE;
	double RANGE_RATE;
	double speed;
	double yawrate;


	rawdata=0x0;
	rawdatalow=0x0;
	rawdatahigh=0x0;
	rawdata1=0x0;
	rawdata2=0x0;
	rawdata3=0x0;

	ANGLE=0;
	RANGE=0;
	RANGE_RATE=0;	

	DWORD timeold;
	CFile file;
	CFileException ex;

	int f_run = 1;
	timeOffset = canReadTimer（hnd）;
	seq=seq+1;
	SYSTEMTIME st;
	GetSystemTime（&st）;
	CString str;
	CString filename;
	str.Format（“Radar_%05d_%02d%02d%02d_%03d.txt“ seq st.wHour st.wMinute st.wSecond st.wMilliseconds）; //设置文件名
//此处需要处理下文件名  将文件名与文件夹名拼接起来
	filename=FilePath+str;
	
	if （!file.Open（filename.operator LPCTSTR（） CFile::modeCreate | CFile::modeWrite   &ex））
	{
		AfxMessageBox（“Can‘t create log file .\n“）;
		canClose（hnd）;
		exit（1）;
	}
	else
	{
		CString str;
		str.Format（“Logging to （candump -h for help; ESC to quit.）\n“）;
	}


	do																					//获得车速信号
	{
		stat = canReadWait（hnd &id msg &dlc &flags &time50）;
	} while （id!=1024）;

	for （int k=1;k<=（64*RADAR_NUM+1）;k++）									//雷达数为3
	{
		if （stat == canOK && （Quiet <= 1））
		{
			if （（flags & canMSG_ERROR_frame） == 0）
			{
				// A message.
				// Print identifier and flags.
				CString str;
				str.Format（“%8lu%c%c%c%c  %02lu “
					id
					flags & canMSG_EXT        ? ‘x‘ : ‘ ‘
					flags & canMSG_RTR        ? ‘R‘ : ‘ ‘
					flags & canMSGERR_OVERRUN ? ‘o‘ : ‘ ‘
					flags & canMSG_NERR       ? ‘N‘ : ‘ ‘ // TJA 1053/1054 transceivers only
					dlc）;
				file.Write（str.operator LPCTSTR（） str.GetLength（））;
			} 
			else
			{
				// An error frame.
				// CANLIB will set the id to something controller
				// dependent.
				CString str;
				str.For