123 lines
1.8 KiB
C++
123 lines
1.8 KiB
C++
#include <string.h>
|
|
#include <stdlib.h>
|
|
|
|
#include "gpio.h"
|
|
|
|
#include "uart.h"
|
|
#include "tick.h"
|
|
|
|
#include "tof.h"
|
|
|
|
#pragma pack(push,1)
|
|
struct HeadTOF
|
|
{
|
|
unsigned short Header; // 0x5959
|
|
unsigned short Distance;
|
|
unsigned short Strength;
|
|
unsigned short Temprature; // Temprature/8 - 256;
|
|
unsigned char Check; // Summ & 0xFF
|
|
};
|
|
#pragma pack(pop)
|
|
|
|
static const unsigned long TOF_Count=8;
|
|
static char TOF_Buffer[TOF_Count];
|
|
static unsigned long TOF_Size=0;
|
|
|
|
static float TOF_Range=0;
|
|
|
|
void TOF_Init()
|
|
{
|
|
UART3_Init(115200);
|
|
}
|
|
|
|
void TOF_Update()
|
|
{
|
|
char c;
|
|
if(UART3_Recv(&c, 1))
|
|
{
|
|
if(c==' ')
|
|
{
|
|
TOF_Size=1;
|
|
TOF_Buffer[0]=c;
|
|
return;
|
|
}
|
|
|
|
if(!TOF_Size) return;
|
|
|
|
if(c=='\r')
|
|
{
|
|
if(TOF_Size<4)
|
|
{
|
|
TOF_Size=0;
|
|
return;
|
|
}
|
|
|
|
TOF_Buffer[TOF_Size]='\0';
|
|
|
|
char* end;
|
|
float range=strtof(TOF_Buffer, &end);
|
|
if(end-TOF_Buffer==TOF_Size) TOF_Range=range;
|
|
else
|
|
range=0;
|
|
TOF_Size=0;
|
|
return;
|
|
}
|
|
|
|
TOF_Buffer[TOF_Size++]=c;
|
|
|
|
if(TOF_Size>=TOF_Count)
|
|
{
|
|
TOF_Size=0;
|
|
return;
|
|
}
|
|
|
|
}
|
|
|
|
}
|
|
|
|
void TOF_Update2()
|
|
{
|
|
static HeadTOF buffer;
|
|
static unsigned long index=0;
|
|
char* data=(char*)&buffer;
|
|
|
|
char c;
|
|
while(UART3_Recv(&c, 1))
|
|
{
|
|
if(index<2)
|
|
{
|
|
if(c==0x59)
|
|
{
|
|
data[index++]=c;
|
|
continue;
|
|
}
|
|
else
|
|
{
|
|
index=0;
|
|
break;
|
|
}
|
|
}
|
|
|
|
data[index++]=c;
|
|
|
|
if(index<sizeof(HeadTOF)) continue;
|
|
|
|
index=0;
|
|
|
|
unsigned char check=0;
|
|
for(int a=0; a<sizeof(HeadTOF)-1; a++) check+=data[a];
|
|
|
|
if(buffer.Check!=check) break;
|
|
|
|
TOF_Range=((float)buffer.Distance)/100;
|
|
|
|
break;
|
|
}
|
|
|
|
}
|
|
|
|
float TOF_GetRange()
|
|
{
|
|
return TOF_Range;
|
|
}
|