add firmware

dev/tele.cpp

@@ -0,0 +1,193 @@
+#include <string.h>
+
+#include "gpio.h"
+
+#include "uart.h"
+#include "tick.h"
+
+#include "tele.h"
+
+enum class RecvModeEnum : unsigned char { Begin, Head, Titles, Data, Auto, SetPID, GetPID, Done };
+
+struct RecvHead
+{
+  RecvHead(){};
+  RecvHead(RecvModeEnum mode, unsigned char size, unsigned char crc)
+  {
+    Mode = mode;
+    DataSize = size;
+    DataCRC8 = crc;
+    CRC8 = (unsigned char)((unsigned char)Mode ^ DataSize ^ DataCRC8);
+  }
+
+  RecvModeEnum Mode;
+  unsigned char DataSize;
+  unsigned char DataCRC8;
+
+  unsigned char CRC8;
+  
+  bool Check()
+  {
+    return CRC8 == (unsigned char)((unsigned char)Mode ^ DataSize ^ DataCRC8);
+  }
+};
+
+unsigned char GetCRC8(const void* arr, int size)
+{
+  unsigned char crc = 0;
+  for (int a = 0; a < size; a++) crc ^= ((unsigned char*)arr)[a];
+  return crc;
+}
+
+static RecvHead Tele_Mode(RecvModeEnum::Begin,0,0);
+
+void TELE_Init()
+{
+  UART3_Init(57600);
+}
+
+static unsigned long TELE_LastTime=0;
+static unsigned long TELE_WaitTimer=0;
+
+static unsigned long TELE_Count=0;
+
+static bool DataAuto=true;
+
+void TELE_Update(const void* info, unsigned long size, unsigned long update)
+{
+  
+  switch(Tele_Mode.Mode)
+  {
+    case RecvModeEnum::Begin:
+    {
+      unsigned char begin;
+      int len=UART3_Recv(&begin, 1);
+      TELE_WaitTimer=TICK_GetCount();
+      if(len && begin==0xAA) Tele_Mode.Mode=RecvModeEnum::Head;
+      break;
+    }
+    
+    case RecvModeEnum::Head:
+    {
+      int len=UART3_Recv(0, 0);
+      if(len<sizeof(RecvHead)) break;
+      
+      RecvHead head;
+      UART3_Recv(&head, sizeof(RecvHead));
+      
+      if(!head.Check()) Tele_Mode.Mode=RecvModeEnum::Begin;
+      else Tele_Mode=head;
+      
+      break;
+    }
+    
+    case RecvModeEnum::Titles:
+    {
+      const char* titles="Acc X|AccY|Acc Z|Acc S|Gyr X|Gyr Y|Gyr Z|Pitch|Roll|Yaw";
+      int len=strlen(titles);
+      
+      unsigned char h=0xAA;
+      UART3_Send(&h, 1);
+      
+      RecvHead head(RecvModeEnum::Titles, len, GetCRC8(titles, len));
+      UART3_Send(&head, sizeof(head));
+      UART3_Send(titles, len);
+      
+      Tele_Mode.Mode=RecvModeEnum::Begin;
+      
+      break;
+    }
+    
+    /*case RecvModeEnum::GetPID:
+    {
+      float send[20]={PID_X.Min, PID_X.Max, 
+      
+      PID_X.P.Min, PID_X.P.Max, PID_X.P.C,
+      PID_X.I.Min, PID_X.I.Max, PID_X.I.C,
+      PID_X.D.Min, PID_X.D.Max, PID_X.D.C,
+      
+      PID_Z.P.Min, PID_Z.P.Max, PID_Z.P.C,
+      PID_Z.I.Min, PID_Z.I.Max, PID_Z.I.C,
+      PID_Z.D.Min, PID_Z.D.Max, PID_Z.D.C
+      };
+      
+      unsigned char h=0xAA;
+      UART3_Send(&h, 1);
+      
+      RecvHead head(RecvModeEnum::GetPID, sizeof(send), GetCRC8(send, sizeof(send)));
+      UART3_Send(&head, sizeof(head));
+      UART3_Send(send, sizeof(send));
+      
+      Tele_Mode.Mode=RecvModeEnum::Begin;
+      
+      break;
+    }*/
+    
+    case RecvModeEnum::Data:
+    {
+      unsigned char h=0xAA;
+      UART3_Send(&h, 1);
+      
+      RecvHead head(RecvModeEnum::Data, size, GetCRC8(info, size));
+      UART3_Send(&head, sizeof(head));
+      UART3_Send(info, size);
+      
+      Tele_Mode.Mode=RecvModeEnum::Begin;
+      
+      break;
+    }
+    
+    /*case RecvModeEnum::SetPID:
+    {
+      float recv[20];
+      
+      unsigned long len=UART3_Recv(0, 0);
+      if(len<sizeof(recv)) break;
+      
+      UART3_Recv(recv, sizeof(recv));
+      
+      unsigned char crc=GetCRC8(recv, sizeof(recv));
+      if(Tele_Mode.DataCRC8!=crc) break;
+      
+      PID_X.Min=recv[0]; PID_X.Max=recv[1];
+      
+      PID_X.P.Min=recv[2]; PID_X.P.Max=recv[3]; PID_X.P.C=recv[4];
+      PID_X.I.Min=recv[5]; PID_X.I.Max=recv[6]; PID_X.I.C=recv[7];
+      PID_X.D.Min=recv[8]; PID_X.D.Max=recv[9]; PID_X.D.C=recv[10];
+      
+      PID_Z.P.Min=recv[11]; PID_Z.P.Max=recv[12]; PID_Z.P.C=recv[13];
+      PID_Z.I.Min=recv[14]; PID_Z.I.Max=recv[15]; PID_Z.I.C=recv[16];
+      PID_Z.D.Min=recv[17]; PID_Z.D.Max=recv[18]; PID_Z.D.C=recv[19];
+      
+      unsigned char h=0xAA;
+      UART3_Send(&h, 1);
+      
+      RecvHead head(RecvModeEnum::Done, 0, 0);
+      UART3_Send(&head, sizeof(head));
+      
+      Tele_Mode.Mode=RecvModeEnum::Begin;
+      
+      break;
+    }*/
+    
+    default:
+    {
+      Tele_Mode.Mode=RecvModeEnum::Begin;
+      break;
+    }
+  }
+  
+  if(TICK_GetCount()-TELE_WaitTimer>100) Tele_Mode.Mode=RecvModeEnum::Begin;
+  
+  if(DataAuto && (TICK_GetCount()-TELE_LastTime>update))
+  {
+    TELE_LastTime=TICK_GetCount();
+    
+    unsigned char h=0xAA;
+    UART3_Send(&h, 1);
+    
+    RecvHead head(RecvModeEnum::Data, size, GetCRC8(info, size));
+    UART3_Send(&head, sizeof(head));
+    UART3_Send(info, size);
+  }
+}