first commit

dev/tof.cpp

@@ -0,0 +1,122 @@
+#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;
+}