串口ok

Common/uart/sbus/sbus.c

@@ -0,0 +1,123 @@
+
+#include "sbus.h"
+
+static bool update(uint8_t Buf[23], uint16_t Channel[18])
+{
+    uint16_t temp;
+    uint16_t ChannelTemp[16];
+
+    ChannelTemp[0]  = ((Buf[2]  & 0x07) << 8) | (Buf[1]);
+    ChannelTemp[1]  = ((Buf[3]  & 0x3F) << 5) | (Buf[2]  >> 3);
+    ChannelTemp[2]  = ((Buf[5]  & 0x01) << 10)| (Buf[4]  << 2) | (Buf[3] >> 6);
+    ChannelTemp[3]  = ((Buf[6]  & 0x0F) << 7) | (Buf[5]  >> 1);
+    ChannelTemp[4]  = ((Buf[7]  & 0x7F) << 4) | (Buf[6]  >> 4);
+    ChannelTemp[5]  = ((Buf[9]  & 0x03) << 9) | (Buf[8]  << 1) | (Buf[7] >> 7);
+    ChannelTemp[6]  = ((Buf[10] & 0x1F) << 6) | (Buf[9]  >> 2);
+    ChannelTemp[7]  = ((Buf[11] & 0xFF) << 3) | (Buf[10] >> 5);
+    ChannelTemp[8]  = ((Buf[13] & 0x07) << 8) | (Buf[12]);
+    ChannelTemp[9] = ((Buf[14] & 0x3F) << 5) | (Buf[13] >> 3);
+    ChannelTemp[10] = ((Buf[16] & 0x01) << 10)| (Buf[15] << 2) | (Buf[14] >> 6);
+    ChannelTemp[11] = ((Buf[17] & 0x0F) << 7) | (Buf[16] >> 1);
+    ChannelTemp[12] = ((Buf[18] & 0x7F) << 4) | (Buf[17] >> 4);
+    ChannelTemp[13] = ((Buf[20] & 0x03) << 9) | (Buf[19] << 1) | (Buf[18] >> 7);
+    ChannelTemp[14] = ((Buf[21] & 0x1F) << 6) | (Buf[20] >> 2);
+    ChannelTemp[15] = ((Buf[22] & 0xFF) << 3) | (Buf[21] >> 5);
+
+    for(int i = 0; i < 16; ++i )
+    {
+        temp = (uint16_t)(0.606f * ChannelTemp[i] + 879.39f);
+        if (temp >=800 && temp <= 2200)
+        {
+            ChannelTemp[i] = temp;
+        }
+        else
+        {
+            return false;
+        }
+    }
+
+    if(Buf[23] & (1<<1))//bit 1
+    {
+      Channel[16] = 1900;
+    }
+    else
+    {
+      Channel[16] = 1100;
+    }
+
+    //DigiChannel 2
+    if(Buf[23] & (1))//bit 0
+    {
+      Channel[17] = 1900;
+    }
+    else
+    {
+      Channel[17] = 1100;
+    }
+
+    if(Buf[23] & 0x0C)//signal is lost/fail/ok?
+    {
+        return false;
+    }
+
+    for(int i = 0; i < 16; ++i )
+    {
+       Channel[i] = ChannelTemp[i];
+		}
+    return true;
+}
+
+
+void StartSBusTask(void * argument)
+{
+	uint8_t buff[32];
+	size_t len;
+
+    SBUS_IN_t *sbus_in = (SBUS_IN_t *)argument;
+    UART_RX_FIFO_t *fifo_rx = sbus_in->fifo_in;
+	  uint16_t *Channel = sbus_in->ch;
+	  uint8_t *seq = &(sbus_in->seq);
+	  uint8_t *valid = &(sbus_in->valid);
+
+	for( ;; )
+	{
+		fifo_rx->task_waitfor_idle = xTaskGetCurrentTaskHandle();
+		ulTaskNotifyTake( pdTRUE, -1);
+    len = UART_RX_FIFO_getlen(fifo_rx);
+		while (len > sizeof(buff))
+		{
+			UART_RX_FIFO_read(fifo_rx, buff, sizeof(buff));
+		  len -= sizeof(buff);
+		}
+		len = UART_RX_FIFO_read(fifo_rx, buff, sizeof(buff));
+		
+		if (len > 24 && buff[0] == 0x0F && ((buff[24] & 0x0F) == 0x04) && ((buff[24] >> 4) < 4))
+		{
+			++(*seq);
+			if (update(buff, Channel))
+			{
+                            *valid = 1u;
+			}
+                        else
+                        {
+                            *valid = 0u;
+                        }
+		}
+	}	
+}
+
+void SBUS_IN_init(SBUS_IN_t *sbus_in, const char *name, UART_RX_FIFO_t *fifo_in)
+{
+    sbus_in->fifo_in = fifo_in;
+    sbus_in->name = name;
+
+    xTaskCreate(
+							  StartSBusTask,
+							  name,
+							  128,
+							  (void *)sbus_in,
+							  tskIDLE_PRIORITY+4,
+							  &(sbus_in->task_p)
+						  );
+}
+